[paparazzi-commits] [4406] Add optional support for timestamps in UDP messages using udp_link. py

[Allen Ibara]

Revision: 4406
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4406
Author:   aibara
Date:     2010-01-05 04:16:31 +0000 (Tue, 05 Jan 2010)
Log Message:
-----------
Add optional support for timestamps in UDP messages using udp_link.py

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/udp_transport.h
    paparazzi3/trunk/sw/ground_segment/python/messages_app/messagesframe.py
    paparazzi3/trunk/sw/ground_segment/python/real_time_plot/messagepicker.py
    paparazzi3/trunk/sw/ground_segment/python/real_time_plot/plotpanel.py
    paparazzi3/trunk/sw/ground_segment/python/udp_link/udp_link.py
    paparazzi3/trunk/sw/lib/python/messages_tool.py
    paparazzi3/trunk/sw/lib/python/messages_xml_map.py

Modified: paparazzi3/trunk/sw/airborne/fms/udp_transport.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/udp_transport.h    2009-12-27 05:04:31 UTC 
(rev 4405)
+++ paparazzi3/trunk/sw/airborne/fms/udp_transport.h    2010-01-05 04:16:31 UTC 
(rev 4406)
@@ -5,14 +5,24 @@
 #include "fms_debug.h"
 #include "std.h"
 
+#ifdef UDP_TRANSPORT_TIMESTAMP
+#define STX_TS  0x98
+#define UdpTransportSizeOf(_payload) (_payload + 8)
+#define UdpTransportPutTimestamp(x) UdpTransportPutUint32ByAddr(x)
+#define UdpTransportPutSTX() UdpTransportPut1Byte(STX_TS)
+#else
 #define STX  0x99
+#define UdpTransportSizeOf(_payload) (_payload + 4)
+#define UdpTransportPutTimestamp(x) {} 
+#define UdpTransportPutSTX() UdpTransportPut1Byte(STX)
+#endif
+
 #define UDPT_TX_BUF_LEN 1496
 extern char updt_tx_buf[UDPT_TX_BUF_LEN];
 extern uint16_t udpt_tx_buf_idx;
 extern uint8_t udpt_ck_a, udpt_ck_b;
 #define  UDPT_TX_BUF_WATERMARK 1024
 
-
 #define UdpTransportPeriodic() {                               \
     if (udpt_tx_buf_idx) {                                     \
       int len;                                                 \
@@ -27,16 +37,18 @@
 
 #define UdpTransportCheckFreeSpace(_x) (TRUE)
 
-#define UdpTransportSizeOf(_payload) (_payload+4)
 
 #define UdpTransportHeader(payload_len) {              \
+    uint32_t msg_timestamp = 0;                                \
     /*udpt_tx_buf_idx = 0;*/                           \
-    UdpTransportPut1Byte(STX);                         \
+    UdpTransportPutSTX();              \
     uint8_t msg_len = UdpTransportSizeOf(payload_len); \
-    UdpTransportPut1Byte(msg_len);                     \
-    udpt_ck_a = msg_len; udpt_ck_b = msg_len;          \
+    udpt_ck_a = udpt_ck_b = 0;                         \
+    UdpTransportPutUint8(msg_len);                     \
+    UdpTransportPutTimestamp(&msg_timestamp);          \
 }
 
+
 #define UdpTransportTrailer() {                                                
\
     UdpTransportPut1Byte(udpt_ck_a);                                   \
     UdpTransportPut1Byte(udpt_ck_b);                                   \

Modified: 
paparazzi3/trunk/sw/ground_segment/python/messages_app/messagesframe.py
===================================================================
--- paparazzi3/trunk/sw/ground_segment/python/messages_app/messagesframe.py     
2009-12-27 05:04:31 UTC (rev 4405)
+++ paparazzi3/trunk/sw/ground_segment/python/messages_app/messagesframe.py     
2010-01-05 04:16:31 UTC (rev 4406)
@@ -80,7 +80,7 @@
 
   def add_new_message(self, aircraft, name):
       messages_book = aircraft.messages_book
-      aircraft.messages[name] = messages_tool.Message(name)
+      aircraft.messages[name] = messages_tool.Message("telemetry", name)
       field_panel = wx.Panel(messages_book)
       grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].field_names), 
2)
 

Modified: 
paparazzi3/trunk/sw/ground_segment/python/real_time_plot/messagepicker.py
===================================================================
--- paparazzi3/trunk/sw/ground_segment/python/real_time_plot/messagepicker.py   
2009-12-27 05:04:31 UTC (rev 4405)
+++ paparazzi3/trunk/sw/ground_segment/python/real_time_plot/messagepicker.py   
2010-01-05 04:16:31 UTC (rev 4406)
@@ -41,7 +41,7 @@
     if not aircraft.messages.has_key(name):
       msg_node = self.tree.AppendItem(ac_node, str(name))
       self.tree.SortChildren(ac_node)
-      aircraft.messages[name] = messages_tool.Message(name)
+      aircraft.messages[name] = messages_tool.Message("telemetry", name)
       for field in aircraft.messages[name].field_names:
         item = self.tree.AppendItem(msg_node, field)
 

Modified: paparazzi3/trunk/sw/ground_segment/python/real_time_plot/plotpanel.py
===================================================================
--- paparazzi3/trunk/sw/ground_segment/python/real_time_plot/plotpanel.py       
2009-12-27 05:04:31 UTC (rev 4405)
+++ paparazzi3/trunk/sw/ground_segment/python/real_time_plot/plotpanel.py       
2010-01-05 04:16:31 UTC (rev 4406)
@@ -252,7 +252,7 @@
 
         for field in self.plots[ac_id][message]:
             plot = self.plots[ac_id][message][field]
-            ix = messages_xml_map.message_dictionary[message].index(field)
+            ix = 
messages_xml_map.message_dictionary["telemetry"][message].index(field)
             point = float(data[ix+2])
 
             if self.x_axis == None or self.x_axis.id != plot.id:

Modified: paparazzi3/trunk/sw/ground_segment/python/udp_link/udp_link.py
===================================================================
--- paparazzi3/trunk/sw/ground_segment/python/udp_link/udp_link.py      
2009-12-27 05:04:31 UTC (rev 4405)
+++ paparazzi3/trunk/sw/ground_segment/python/udp_link/udp_link.py      
2010-01-05 04:16:31 UTC (rev 4406)
@@ -7,20 +7,33 @@
 import logging
 import sys
 import threading
+import time
 
 sys.path.append(os.getenv("PAPARAZZI_HOME") + "/sw/lib/python")
 
 import messages_xml_map
 
-class IvyUdpLink():
-    def __init__(self):
-      self.InitIvy()
-      self.status_timer = threading.Timer(1.0, self.sendStatus)
-      self.run_time = 0
+PING_PERIOD = 5.0
+STATUS_PERIOD = 1.0
+
+class DownLinkStatus():
+  def __init__(self, ac_id, address):
+      self.ac_id = ac_id
+      self.address = address
       self.rx_bytes = 0
       self.rx_msgs = 0
+      self.run_time = 0
       self.last_rx_bytes = 0
       self.last_rx_msgs = 0
+      self.last_ping_time = 0
+      self.last_pong_time = 0
+
+class IvyUdpLink():
+    def __init__(self):
+      self.InitIvy()
+      self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus)
+      self.ping_timer = threading.Timer(STATUS_PERIOD, self.sendPing)
+      self.ac_downlink_status = { }
       self.rx_err = 0
 
       messages_xml_map.ParseMessages()
@@ -36,23 +49,6 @@
     def Unpack(self, data_fields, type, start, length):
       return struct.unpack(type, "".join(data_fields[start:start + length]))[0]
 
-    def ProcessMessage(self, message_values, fromRemote):
-      # Extract aircraft id from message and ignore if not matching
-      msg_ac_id = int(message_values[0])
-      if (msg_ac_id != self.ac_ids[0]):
-        return
-
-      # Extract setting value
-      setting_index = int(message_values[1])
-      setting_value = message_values[2]
-
-    # Called for DL_VALUE (from aircraft)
-    def OnValueMsg(self, agent, *larg):
-      # Extract field values
-      message_values = larg[0].split(' ')
-      message_values = message_values[0:1] + message_values[2:]
-      self.ProcessMessage(message_values, True)
-
     def InitIvy(self):
       # initialising the bus
       IvyInit("Link", # application name for Ivy
@@ -63,27 +59,87 @@
                        )
 
       # starting the bus
-      logging.getLogger('Ivy').setLevel(logging.DEBUG)
+      logging.getLogger('Ivy').setLevel(logging.WARN)
       IvyStart("")
-      #IvyBindMsg(self.OnValueMsg, "(^.* DL_VALUE .*)")
+      IvyBindMsg(self.OnSettingMsg, "(^.* SETTING .*)")
 
+    def calculate_checksum(self, msg):
+      ck_a = 0
+      ck_b = 0
+      # start char not included in checksum for pprz protocol
+      for c in msg[1:]:
+       ck_a = (ck_a + ord(c)) % 256
+       ck_b = (ck_b + ck_a) % 256
+      return (ck_a, ck_b)
+
+    def buildPprzMsg(self, msg_id, *args):
+      stx = 0x99
+      length = 6
+      sender = 0
+      msg_fields = 
messages_xml_map.message_dictionary_types["datalink"][msg_id]
+      struct_string = "=BBBB"
+      typed_args = []
+      idx = 0
+      for msg_type in msg_fields:
+       struct_string += self.data_types[msg_type][0]
+       length += self.data_types[msg_type][1]
+       typed_args.append(args[idx])
+      msg = struct.pack(struct_string, stx, length, sender, msg_id, *args)
+      (ck_a, ck_b) = self.calculate_checksum(msg)
+      msg = msg + struct.pack('=BB', ck_a, ck_b)
+      return msg
+      
+    def OnSettingMsg(self, agent, *larg):
+      list = larg[0].split(' ')
+      sender = list[0]
+      msg_name = list[1]
+      ac_id = list[3]
+      args = list[2:]
+      msg_id = 
messages_xml_map.message_dictionary_name_id["datalink"][msg_name]
+      if self.ac_downlink_status.has_key(int(ac_id)):
+       msgbuf = self.buildPprzMsg(msg_id, *args)
+       address = (self.ac_downlink_status[int(ac_id)].address[0], 4243)
+       self.server.sendto(msgbuf, address)
+
+    def sendPing(self):
+      for (ac_id, value) in self.ac_downlink_status.items():
+       msg_id = messages_xml_map.message_dictionary_name_id["datalink"]["PING"]
+       msgbuf = self.buildPprzMsg(msg_id)
+       address = (self.ac_downlink_status[int(ac_id)].address[0], 4243)
+       self.server.sendto(msgbuf, address)
+       value.last_ping_time = time.clock()
+      
+      self.ping_timer = threading.Timer(STATUS_PERIOD, self.sendPing)
+      self.ping_timer.start()
+
     def sendStatus(self):
-      self.run_time = self.run_time + 1
-      IvySendMsg("11 DOWNLINK_STATUS %i %i %i %i %i %i %i" % (
-        self.run_time,
-        self.rx_bytes - self.last_rx_bytes,
-        self.rx_msgs - self.last_rx_msgs,
-        self.rx_err,
-        self.rx_bytes,
-        self.rx_msgs,
-        0 ))
-      self.last_rx_bytes = self.rx_bytes
-      self.last_rx_msgs = self.rx_msgs
+      for (key, value) in self.ac_downlink_status.items():
+       IvySendMsg("%i DOWNLINK_STATUS %i %i %i %i %i %i %i" % (
+         value.ac_id,
+         value.run_time,
+         value.rx_bytes,
+         value.rx_msgs,
+         self.rx_err,
+         value.rx_bytes - value.last_rx_bytes,
+         value.rx_msgs - value.last_rx_msgs,
+         1000 * value.last_pong_time ))
+       value.last_rx_bytes = value.rx_bytes
+       value.last_rx_msgs = value.rx_msgs
+       value.run_time = value.run_time + 1
 
-      self.status_timer = threading.Timer(1.0, self.sendStatus)
+      self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus)
       self.status_timer.start()
 
-    def ProcessPacket(self, msg):
+    def updateStatus(self, ac_id, length, address, isPong):
+      if not self.ac_downlink_status.has_key(ac_id):
+       self.ac_downlink_status[ac_id] = DownLinkStatus(ac_id, address)
+
+      self.ac_downlink_status[ac_id].rx_msgs += 1
+      self.ac_downlink_status[ac_id].rx_bytes += length
+      if isPong:
+       self.ac_downlink_status[ac_id].last_pong_time = time.clock() - 
self.ac_downlink_status[ac_id].last_ping_time 
+
+    def ProcessPacket(self, msg, address):
       if len(msg) < 4:
         self.rx_err = self.rx_err + 1
         return
@@ -91,17 +147,19 @@
       msg_offset = 0
       while msg_offset < len(msg):
        start_byte = ord(msg[msg_offset])
+       msg_start_idx = msg_offset
        msg_offset = msg_offset + 1
 
-       if start_byte != 0x99:
+       if start_byte != 0x99 and start_byte != 0x98:
          self.rx_err = self.rx_err + 1
          return
 
        msg_length = ord(msg[msg_offset])
        msg_offset = msg_offset + 1
 
-       #timestamp = int(self.Unpack(msg, 'L', msg_offset, 4))
-       #msg_offset = msg_offset + 4
+       if (start_byte == 0x98):
+         timestamp = int(self.Unpack(msg, 'L', msg_offset, 4))
+         msg_offset = msg_offset + 4
 
        ac_id = ord(msg[msg_offset])
        msg_offset = msg_offset + 1
@@ -109,8 +167,8 @@
         msg_id = ord(msg[msg_offset])
         msg_offset = msg_offset + 1
       
-        msg_name = messages_xml_map.message_dictionary_id_name[msg_id]
-        msg_fields = messages_xml_map.message_dictionary_types[msg_id]
+        msg_name = 
messages_xml_map.message_dictionary_id_name["telemetry"][msg_id]
+        msg_fields = 
messages_xml_map.message_dictionary_types["telemetry"][msg_id]
 
         ivy_msg = "%i %s " % (ac_id, msg_name)
 
@@ -134,24 +192,29 @@
            print "finished without parsing %s" % field
            break
 
-       msg_offset += 2 # munch munch checksum bytes
-  
-        self.rx_msgs = self.rx_msgs + 1
-        self.rx_bytes = self.rx_bytes + len(msg)
-       ivy_msg = ivy_msg[:-1]
-        IvySendMsg(ivy_msg)
+       (ck_a, ck_b) = self.calculate_checksum(msg[msg_start_idx:msg_offset])
+       msg_ck_a = int(self.Unpack(msg, 'B', msg_offset, 1))
+       msg_offset += 1
+       msg_ck_b = int(self.Unpack(msg, 'B', msg_offset, 1))
+       msg_offset += 1
 
+       # check for valid checksum
+       if (ck_a, ck_b) == (msg_ck_a, msg_ck_b):
+         self.updateStatus(ac_id, msg_length, address, msg_id == 
messages_xml_map.message_dictionary_name_id["telemetry"]["PONG"])
 
+         # strip off trailing whitespace
+         ivy_msg = ivy_msg[:-1]
+         IvySendMsg(ivy_msg)
+
     def Run(self):
-      server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
-      server.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
-      msg_count = 0
-      server.bind(('0.0.0.0' , 4242))
+      self.server = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+      self.server.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+      self.server.bind(('0.0.0.0' , 4242))
       self.status_timer.start()
+      self.ping_timer.start()
       while True:
-        msg = server.recv(2048)
-        msg_count = msg_count + 1
-        self.ProcessPacket(msg)
+        (msg, address) = self.server.recvfrom(2048)
+        self.ProcessPacket(msg, address)
 
 def main():
   udp_interface = IvyUdpLink()

Modified: paparazzi3/trunk/sw/lib/python/messages_tool.py
===================================================================
--- paparazzi3/trunk/sw/lib/python/messages_tool.py     2009-12-27 05:04:31 UTC 
(rev 4405)
+++ paparazzi3/trunk/sw/lib/python/messages_tool.py     2010-01-05 04:16:31 UTC 
(rev 4406)
@@ -5,10 +5,10 @@
 import os
 
 class Message:
-  def __init__(self, name):
+  def __init__(self, class_name, name):
     messages_xml_map.ParseMessages()
     self.field_value = []
-    self.field_names = messages_xml_map.message_dictionary[name]
+    self.field_names = messages_xml_map.message_dictionary[class_name][name]
     self.field_controls = []
     self.index = None
     self.last_seen = time.clock()

Modified: paparazzi3/trunk/sw/lib/python/messages_xml_map.py
===================================================================
--- paparazzi3/trunk/sw/lib/python/messages_xml_map.py  2009-12-27 05:04:31 UTC 
(rev 4405)
+++ paparazzi3/trunk/sw/lib/python/messages_xml_map.py  2010-01-05 04:16:31 UTC 
(rev 4406)
@@ -9,6 +9,7 @@
 message_dictionary = {}
 message_dictionary_types = {}
 message_dictionary_id_name = {}
+message_dictionary_name_id = {}
 
 def Usage(scmd):
        lpathitem = scmd.split('/')
@@ -37,7 +38,14 @@
 def ParseMessages():
   from lxml import etree
   tree = etree.parse( messages_path)
-  for the_message in tree.xpath("//address@hidden'telemetry']/address@hidden"):
+  for the_class in tree.xpath("//address@hidden"):
+    class_name = the_class.attrib['name']
+    if not message_dictionary.has_key(class_name):
+      message_dictionary_id_name[class_name] = {}
+      message_dictionary_name_id[class_name] = {}
+      message_dictionary[class_name] = {}
+      message_dictionary_types[class_name] = {}
+    for the_message in the_class.xpath("address@hidden"):
       message_name = the_message.attrib['name']
       if the_message.attrib.has_key('id'):
         message_id = the_message.attrib['id']
@@ -48,28 +56,22 @@
       else:
         message_id = int(message_id)
 
-      message_dictionary_id_name[message_id] = message_name
+      message_dictionary_id_name[class_name][message_id] = message_name
+      message_dictionary_name_id[class_name][message_name] = message_id
 
       # insert this message into our dictionary as a list with room for the 
fields
-      message_dictionary[message_name] = []
-      message_dictionary_types[message_id] = []
+      message_dictionary[class_name][message_name] = []
+      message_dictionary_types[class_name][message_id] = []
 
       for the_field in the_message.xpath('address@hidden'):
         # for now, just save the field names -- in the future maybe expand 
this to save a struct?
-        message_dictionary[message_name].append( the_field.attrib['name'])
-        message_dictionary_types[message_id].append( the_field.attrib['type'])
+        message_dictionary[class_name][message_name].append( 
the_field.attrib['name'])
+        message_dictionary_types[class_name][message_id].append( 
the_field.attrib['type'])
     
 def test():
   GetOptions()
   ParseMessages()
-  print message_dictionary['WHIRLY']
-  print message_dictionary['WHIRLY'].index('plane2_pitch')
-  print message_dictionary['ACTUATORS']
-  print message_dictionary['ACTUATORS'].index('actuator0')
-  print message_dictionary_types[53]
-  print message_dictionary_id_name[53]
   
-  
 if __name__ == '__main__':
   test()