mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
[pprzlink] allow to compile paparazzi with PPRZLINK v2 (#2083)
To activate, build project with correct option: PPRZLINK_LIB_VERSION=2.0 make (note that 'make clean' is usualy required before that)
This commit is contained in:
Gautier Hattenberger
committed by
Michal Podhradsky
Michal Podhradsky
parent
2aabe921b3
commit
6a72c5da84
@@ -78,6 +78,11 @@ LOGALIZER=sw/logalizer
|
||||
|
||||
SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) sw/tools
|
||||
|
||||
#
|
||||
# Communication protocol version
|
||||
#
|
||||
PPRZLINK_LIB_VERSION ?= 1.0
|
||||
|
||||
#
|
||||
# xml files used as input for header generation
|
||||
#
|
||||
@@ -91,7 +96,7 @@ XSENS_XML = $(CONF)/xsens_MTi-G.xml
|
||||
# generated header files
|
||||
#
|
||||
PPRZLINK_DIR=sw/ext/pprzlink
|
||||
PPRZLINK_INSTALL=$(PAPARAZZI_HOME)/var/lib/ocaml
|
||||
PPRZLINK_INSTALL=$(PAPARAZZI_HOME)/var/lib
|
||||
MESSAGES_INSTALL=$(PAPARAZZI_HOME)/var
|
||||
UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h
|
||||
MTK_PROTOCOL_H=$(STATICINCLUDE)/mtk_protocol.h
|
||||
@@ -137,7 +142,7 @@ static: cockpit tmtc generators sim_static joystick static_h
|
||||
|
||||
libpprzlink:
|
||||
$(MAKE) -C $(EXT) pprzlink.update
|
||||
$(Q)Q=$(Q) DESTDIR=$(PPRZLINK_INSTALL) $(MAKE) -C $(PPRZLINK_DIR) libpprzlink-install
|
||||
$(Q)Q=$(Q) DESTDIR=$(PPRZLINK_INSTALL) PPRZLINK_LIB_VERSION=${PPRZLINK_LIB_VERSION} $(MAKE) -C $(PPRZLINK_DIR) libpprzlink-install
|
||||
|
||||
libpprz: libpprzlink _save_build_version
|
||||
$(MAKE) -C $(LIB)/ocaml
|
||||
@@ -191,10 +196,10 @@ pprzlink_protocol :
|
||||
$(Q)test -d $(STATICLIB) || mkdir -p $(STATICLIB)
|
||||
ifeq ("$(wildcard $(CUSTOM_MESSAGES_XML))","")
|
||||
@echo GENERATE $@ with default messages
|
||||
$(Q)Q=$(Q) MESSAGES_INSTALL=$(MESSAGES_INSTALL) VALIDATE_XML=FALSE $(MAKE) -C $(PPRZLINK_DIR) pymessages
|
||||
$(Q)Q=$(Q) MESSAGES_INSTALL=$(MESSAGES_INSTALL) VALIDATE_XML=FALSE PPRZLINK_LIB_VERSION=${PPRZLINK_LIB_VERSION} $(MAKE) -C $(PPRZLINK_DIR) pymessages
|
||||
else
|
||||
@echo GENERATE $@ with custome messages from $(CUSTOM_MESSAGES_XML)
|
||||
$(Q)Q=$(Q) MESSAGES_XML=$(CUSTOM_MESSAGES_XML) MESSAGES_INSTALL=$(MESSAGES_INSTALL) $(MAKE) -C $(PPRZLINK_DIR) pymessages
|
||||
$(Q)Q=$(Q) MESSAGES_XML=$(CUSTOM_MESSAGES_XML) MESSAGES_INSTALL=$(MESSAGES_INSTALL) PPRZLINK_LIB_VERSION=${PPRZLINK_LIB_VERSION} $(MAKE) -C $(PPRZLINK_DIR) pymessages
|
||||
endif
|
||||
|
||||
|
||||
|
||||
@@ -66,81 +66,121 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/* parse telemetry messages coming from ground station */
|
||||
switch (msg_id) {
|
||||
case DL_PING: {
|
||||
pprz_msg_send_PONG(trans, dev, AC_ID);
|
||||
}
|
||||
break;
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
// Check that the message is really a datalink message
|
||||
if (pprzlink_get_msg_class_id(buf) == DL_datalink_CLASS_ID) {
|
||||
#endif
|
||||
/* parse datalink messages coming from ground station */
|
||||
switch (msg_id) {
|
||||
case DL_PING: {
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
// Reply to the sender of the message
|
||||
struct pprzlink_msg msg;
|
||||
msg.trans = trans;
|
||||
msg.dev = dev;
|
||||
msg.sender_id = AC_ID;
|
||||
msg.receiver_id = sender_id;
|
||||
msg.component_id = 0;
|
||||
pprzlink_msg_send_PONG(&msg);
|
||||
#else
|
||||
pprz_msg_send_PONG(trans, dev, AC_ID);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
case DL_SETTING : {
|
||||
if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
|
||||
uint8_t i = DL_SETTING_index(buf);
|
||||
float var = DL_SETTING_value(buf);
|
||||
DlSetting(i, var);
|
||||
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
|
||||
}
|
||||
break;
|
||||
case DL_SETTING : {
|
||||
if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
|
||||
uint8_t i = DL_SETTING_index(buf);
|
||||
float var = DL_SETTING_value(buf);
|
||||
DlSetting(i, var);
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
// Reply to the sender of the message
|
||||
struct pprzlink_msg msg;
|
||||
msg.trans = trans;
|
||||
msg.dev = dev;
|
||||
msg.sender_id = AC_ID;
|
||||
msg.receiver_id = sender_id;
|
||||
msg.component_id = 0;
|
||||
pprzlink_msg_send_DL_VALUE(&msg, &i, &var);
|
||||
#else
|
||||
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
case DL_GET_SETTING : {
|
||||
if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
|
||||
uint8_t i = DL_GET_SETTING_index(buf);
|
||||
float val = settings_get_value(i);
|
||||
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
|
||||
}
|
||||
break;
|
||||
case DL_GET_SETTING : {
|
||||
if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
|
||||
uint8_t i = DL_GET_SETTING_index(buf);
|
||||
float val = settings_get_value(i);
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
// Reply to the sender of the message
|
||||
struct pprzlink_msg msg;
|
||||
msg.trans = trans;
|
||||
msg.dev = dev;
|
||||
msg.sender_id = AC_ID;
|
||||
msg.receiver_id = sender_id;
|
||||
msg.component_id = 0;
|
||||
pprzlink_msg_send_DL_VALUE(&msg, &i, &val);
|
||||
#else
|
||||
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
|
||||
#ifdef RADIO_CONTROL_TYPE_DATALINK
|
||||
case DL_RC_3CH :
|
||||
#ifdef RADIO_CONTROL_DATALINK_LED
|
||||
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
|
||||
#endif
|
||||
parse_rc_3ch_datalink(
|
||||
DL_RC_3CH_throttle_mode(buf),
|
||||
DL_RC_3CH_roll(buf),
|
||||
DL_RC_3CH_pitch(buf));
|
||||
break;
|
||||
case DL_RC_4CH :
|
||||
if (DL_RC_4CH_ac_id(buf) == AC_ID) {
|
||||
case DL_RC_3CH :
|
||||
#ifdef RADIO_CONTROL_DATALINK_LED
|
||||
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
|
||||
#endif
|
||||
parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
|
||||
DL_RC_4CH_throttle(buf),
|
||||
DL_RC_4CH_roll(buf),
|
||||
DL_RC_4CH_pitch(buf),
|
||||
DL_RC_4CH_yaw(buf));
|
||||
}
|
||||
break;
|
||||
parse_rc_3ch_datalink(
|
||||
DL_RC_3CH_throttle_mode(buf),
|
||||
DL_RC_3CH_roll(buf),
|
||||
DL_RC_3CH_pitch(buf));
|
||||
break;
|
||||
case DL_RC_4CH :
|
||||
if (DL_RC_4CH_ac_id(buf) == AC_ID) {
|
||||
#ifdef RADIO_CONTROL_DATALINK_LED
|
||||
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
|
||||
#endif
|
||||
parse_rc_4ch_datalink(DL_RC_4CH_mode(buf),
|
||||
DL_RC_4CH_throttle(buf),
|
||||
DL_RC_4CH_roll(buf),
|
||||
DL_RC_4CH_pitch(buf),
|
||||
DL_RC_4CH_yaw(buf));
|
||||
}
|
||||
break;
|
||||
#endif // RADIO_CONTROL_TYPE_DATALINK
|
||||
|
||||
#if USE_GPS
|
||||
case DL_GPS_INJECT : {
|
||||
// Check if the GPS is for this AC
|
||||
if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
|
||||
case DL_GPS_INJECT : {
|
||||
// Check if the GPS is for this AC
|
||||
if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
|
||||
|
||||
// GPS parse data
|
||||
gps_inject_data(
|
||||
DL_GPS_INJECT_packet_id(buf),
|
||||
DL_GPS_INJECT_data_length(buf),
|
||||
DL_GPS_INJECT_data(buf)
|
||||
);
|
||||
}
|
||||
break;
|
||||
// GPS parse data
|
||||
gps_inject_data(
|
||||
DL_GPS_INJECT_packet_id(buf),
|
||||
DL_GPS_INJECT_data_length(buf),
|
||||
DL_GPS_INJECT_data(buf)
|
||||
);
|
||||
}
|
||||
break;
|
||||
#if USE_GPS_UBX_RTCM
|
||||
case DL_RTCM_INJECT : {
|
||||
// GPS parse data
|
||||
gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
|
||||
DL_RTCM_INJECT_data_length(buf),
|
||||
DL_RTCM_INJECT_data(buf));
|
||||
}
|
||||
break;
|
||||
case DL_RTCM_INJECT : {
|
||||
// GPS parse data
|
||||
gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
|
||||
DL_RTCM_INJECT_data_length(buf),
|
||||
DL_RTCM_INJECT_data(buf));
|
||||
}
|
||||
break;
|
||||
#endif // USE_GPS_UBX_RTCM
|
||||
#endif // USE_GPS
|
||||
|
||||
default:
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
#if PPRZLINK_DEFAULT_VER == 2
|
||||
}
|
||||
#endif
|
||||
}
|
||||
/* Parse firmware specific datalink */
|
||||
firmware_parse_msg(dev, trans, buf);
|
||||
|
||||
@@ -36,10 +36,6 @@
|
||||
#include "std.h"
|
||||
#include "pprzlink/dl_protocol.h"
|
||||
|
||||
/* Message id helpers */
|
||||
#define SenderIdOfPprzMsg(x) (x[0])
|
||||
#define IdOfPprzMsg(x) (x[1])
|
||||
|
||||
/** Datalink kinds */
|
||||
#define PPRZ 1
|
||||
#define XBEE 2
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: 2e0309c3e3...42e0492bf1
@@ -1,155 +1,147 @@
|
||||
#!/usr/bin/env python
|
||||
|
||||
from ivy.std_api import *
|
||||
import socket
|
||||
import struct
|
||||
import os
|
||||
import logging
|
||||
import sys
|
||||
import threading
|
||||
import time
|
||||
|
||||
# if PAPARAZZI_SRC not set, then assume the tree containing this
|
||||
# file is a reasonable substitute
|
||||
PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
|
||||
'../../../../')))
|
||||
sys.path.append(PPRZ_SRC + "/sw/lib/python")
|
||||
sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
|
||||
PAPARAZZI_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),'../../../../')))
|
||||
sys.path.append(PAPARAZZI_HOME + "/var/lib/python")
|
||||
|
||||
import pprz_env
|
||||
from pprzlink import messages_xml_map
|
||||
from ivy.std_api import *
|
||||
import threading
|
||||
import time
|
||||
|
||||
import pprzlink.udp
|
||||
import pprzlink.ivy
|
||||
import pprzlink.messages_xml_map as messages_xml_map
|
||||
import pprzlink.message as message
|
||||
|
||||
DEFAULT_BROADCAST= "127.255.255.255"
|
||||
REPEAT="Repeat"
|
||||
IP_BCAST="IPBcast"
|
||||
|
||||
PING_PERIOD = 5.0
|
||||
STATUS_PERIOD = 1.0
|
||||
|
||||
STX = 0x99
|
||||
STX_TS = 0x98
|
||||
|
||||
DATALINK_PORT = 4243
|
||||
DOWNLINK_PORT = 4242
|
||||
|
||||
|
||||
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.tx_msgs = 0
|
||||
self.run_time = 0
|
||||
self.last_rx_bytes = 0
|
||||
self.last_rx_msgs = 0
|
||||
self.last_msg_time = 0
|
||||
self.last_ping_time = 0
|
||||
self.last_pong_time = 0
|
||||
|
||||
|
||||
class IvyUdpLink():
|
||||
def __init__(self):
|
||||
self.InitIvy()
|
||||
self.ivy_id = 0
|
||||
self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus)
|
||||
self.ping_timer = threading.Timer(STATUS_PERIOD, self.sendPing)
|
||||
class UDPLink:
|
||||
def __init__(self,opts):
|
||||
messages_xml_map.parse_messages()
|
||||
self.run_event = threading.Event()
|
||||
self.uplink_port = opts.uplink_port
|
||||
self.downlink_port = opts.downlink_port
|
||||
self.udp = pprzlink.udp.UdpMessagesInterface(self.proccess_downlink_msg, False, self.uplink_port, self.downlink_port)
|
||||
self.ivy = pprzlink.ivy.IvyMessagesInterface("UDPLink", True, False, opts.bus)
|
||||
self.ac_downlink_status = {}
|
||||
self.rx_err = 0
|
||||
self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus)
|
||||
self.ping_timer = threading.Timer(PING_PERIOD, self.sendPing)
|
||||
self.bcast_method = opts.broadcast_method
|
||||
self.bcast_addr = opts.broadcast_address
|
||||
|
||||
messages_xml_map.parse_messages()
|
||||
self.data_types = {'float': ['f', 4],
|
||||
'uint8': ['B', 1],
|
||||
'uint16': ['H', 2],
|
||||
'uint32': ['L', 4],
|
||||
'int8': ['b', 1],
|
||||
'int16': ['h', 2],
|
||||
'int32': ['l', 4]
|
||||
}
|
||||
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
|
||||
self.ac_downlink_status[ac_id].last_msg_time = time.time()
|
||||
if isPong:
|
||||
self.ac_downlink_status[ac_id].last_pong_time = time.time() - self.ac_downlink_status[ac_id].last_ping_time
|
||||
|
||||
def __del__(self):
|
||||
self.stop()
|
||||
def proccess_downlink_msg(self,sender,address,msg,length,receiver_id=None, component_id=None):
|
||||
if self.run_event.is_set():
|
||||
# print("new message from %i (%s) [%d Bytes]: %s" % (sender, address, length, msg))
|
||||
self.ivy.send(msg,sender,receiver_id,component_id)
|
||||
self.updateStatus(sender, length, address,msg.name == "PONG")
|
||||
|
||||
def stop(self):
|
||||
self.status_timer.cancel()
|
||||
self.ping_timer.cancel()
|
||||
IvyUnBindMsg(self.ivy_id)
|
||||
IvyStop()
|
||||
|
||||
def Unpack(self, data_fields, type, start, length):
|
||||
return struct.unpack(type, "".join(data_fields[start:start + length]))[0]
|
||||
|
||||
def InitIvy(self):
|
||||
# initialising the bus
|
||||
IvyInit("Link", # application name for Ivy
|
||||
"READY", # ready message
|
||||
0, # main loop is local (ie. using IvyMainloop)
|
||||
lambda x, y: y, # handler called on connection/deconnection
|
||||
lambda x, y: y # handler called when a diemessage is received
|
||||
)
|
||||
|
||||
# starting the bus
|
||||
logging.getLogger('Ivy').setLevel(logging.WARN)
|
||||
IvyStart(pprz_env.IVY_BUS)
|
||||
self.ivy_id = 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 = STX
|
||||
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]
|
||||
if (msg_type == "float"):
|
||||
typed_args.append(float(args[idx]))
|
||||
def proccess_uplink_msg(self,ac_id,msg):
|
||||
# print ('New IVY message to %s : %s' % (ac_id,msg))
|
||||
if msg.broadcasted:
|
||||
if self.bcast_method==IP_BCAST:
|
||||
self.udp.send(msg,0,(self.bcast_addr,self.uplink_port))
|
||||
else:
|
||||
typed_args.append(int(args[idx]))
|
||||
idx += 1
|
||||
msg = struct.pack(struct_string, stx, length, sender, msg_id, *typed_args)
|
||||
(ck_a, ck_b) = self.calculate_checksum(msg)
|
||||
msg = msg + struct.pack('=BB', ck_a, ck_b)
|
||||
return msg
|
||||
for dest in self.ac_downlink_status.keys():
|
||||
self.udp.send(msg, 0, (self.ac_downlink_status[dest].address[0], self.uplink_port))
|
||||
self.ac_downlink_status[dest].tx_msgs += 1
|
||||
else:
|
||||
if isinstance(ac_id,str):
|
||||
ac_id = int(ac_id)
|
||||
# Only send message if the ac is known
|
||||
if self.ac_downlink_status.has_key(ac_id):
|
||||
self.udp.send(msg,0,(self.ac_downlink_status[ac_id].address[0],self.uplink_port),ac_id)
|
||||
self.ac_downlink_status[ac_id].tx_msgs+=1
|
||||
else:
|
||||
print ('Message for unknown ac %d' % ac_id)
|
||||
|
||||
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], DATALINK_PORT)
|
||||
self.server.sendto(msgbuf, address)
|
||||
def initial_ivy_binds(self):
|
||||
# Subscribe to all datalink messages
|
||||
messages_datalink = messages_xml_map.get_msgs("datalink")
|
||||
for msg in messages_datalink:
|
||||
self.ivy.subscribe(self.proccess_uplink_msg, message.PprzMessage("datalink", msg))
|
||||
|
||||
def run(self):
|
||||
print ('Starting UDPLink for protocol version %s' % (messages_xml_map.PROTOCOL_VERSION))
|
||||
self.udp.start()
|
||||
self.ivy.start()
|
||||
|
||||
self.run_event.set()
|
||||
|
||||
self.status_timer.start()
|
||||
self.ping_timer.start()
|
||||
|
||||
self.initial_ivy_binds()
|
||||
|
||||
try:
|
||||
while True:
|
||||
time.sleep(.5)
|
||||
except KeyboardInterrupt:
|
||||
print ("Stopping UDPLink.")
|
||||
self.status_timer.cancel()
|
||||
self.ping_timer.cancel()
|
||||
self.run_event.clear()
|
||||
# t.join()
|
||||
self.udp.stop()
|
||||
self.ivy.stop()
|
||||
self.udp.join()
|
||||
|
||||
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], DATALINK_PORT)
|
||||
self.server.sendto(msgbuf, address)
|
||||
value.last_ping_time = time.clock()
|
||||
|
||||
self.ping_timer = threading.Timer(STATUS_PERIOD, self.sendPing)
|
||||
if messages_xml_map.PROTOCOL_VERSION=="2.0":
|
||||
# For pprzlink V2.0 set the receiver id
|
||||
self.udp.send(message.PprzMessage('datalink','PING'), 0, self.ac_downlink_status[int(ac_id)].address, ac_id)
|
||||
else:
|
||||
self.udp.send(message.PprzMessage('datalink','PING'),0,self.ac_downlink_status[int(ac_id)].address)
|
||||
value.last_ping_time = time.time()
|
||||
self.ping_timer = threading.Timer(PING_PERIOD, self.sendPing)
|
||||
self.ping_timer.start()
|
||||
|
||||
def sendStatus(self):
|
||||
for (key, value) in self.ac_downlink_status.items():
|
||||
IvySendMsg("%i DOWNLINK_STATUS %i %i %i %i %i %i %i" % (
|
||||
self.ivy.send("link LINK_REPORT %i %i %i %i %i %i %i %i %i %i %i" % (
|
||||
value.ac_id,
|
||||
-1,
|
||||
value.run_time,
|
||||
time.time() - value.last_msg_time,
|
||||
value.rx_bytes,
|
||||
value.rx_msgs,
|
||||
self.rx_err,
|
||||
value.rx_bytes - value.last_rx_bytes,
|
||||
value.rx_msgs - value.last_rx_msgs,
|
||||
value.tx_msgs,
|
||||
1000 * value.last_pong_time))
|
||||
value.last_rx_bytes = value.rx_bytes
|
||||
value.last_rx_msgs = value.rx_msgs
|
||||
@@ -158,104 +150,16 @@ class IvyUdpLink():
|
||||
self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus)
|
||||
self.status_timer.start()
|
||||
|
||||
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
|
||||
|
||||
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 != STX and start_byte != STX_TS:
|
||||
self.rx_err = self.rx_err + 1
|
||||
return
|
||||
|
||||
msg_length = ord(msg[msg_offset])
|
||||
msg_offset = msg_offset + 1
|
||||
|
||||
if (start_byte == STX_TS):
|
||||
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
|
||||
|
||||
msg_id = ord(msg[msg_offset])
|
||||
msg_offset = msg_offset + 1
|
||||
|
||||
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)
|
||||
|
||||
for field in msg_fields:
|
||||
if field[-2:] == "[]":
|
||||
baseType = field[:-2]
|
||||
array_length = int(self.Unpack(msg, 'B', msg_offset, 1))
|
||||
msg_offset = msg_offset + 1
|
||||
for count in range(0, array_length):
|
||||
array_value = str(
|
||||
self.Unpack(msg, self.data_types[baseType][0], msg_offset, self.data_types[baseType][1]))
|
||||
msg_offset = msg_offset + self.data_types[baseType][1]
|
||||
if (count == array_length - 1):
|
||||
ivy_msg += array_value + " "
|
||||
else:
|
||||
ivy_msg += array_value + ","
|
||||
else:
|
||||
ivy_msg += str(
|
||||
self.Unpack(msg, self.data_types[field][0], msg_offset, self.data_types[field][1])) + " "
|
||||
msg_offset = msg_offset + self.data_types[field][1]
|
||||
|
||||
if (msg_offset > len(msg)):
|
||||
print "finished without parsing %s" % field
|
||||
break
|
||||
|
||||
(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):
|
||||
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', DOWNLINK_PORT))
|
||||
self.status_timer.start()
|
||||
self.ping_timer.start()
|
||||
try:
|
||||
while True:
|
||||
(msg, address) = self.server.recvfrom(2048)
|
||||
self.ProcessPacket(msg, address)
|
||||
except KeyboardInterrupt:
|
||||
print("Stopping server on request")
|
||||
|
||||
|
||||
def main():
|
||||
udp_interface = IvyUdpLink()
|
||||
udp_interface.Run()
|
||||
udp_interface.stop()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
from argparse import ArgumentParser
|
||||
|
||||
parser = ArgumentParser(description="UDP link for paparazzi (python version)")
|
||||
parser.add_argument("--broadcast_method", choices=[IP_BCAST,REPEAT] , default=IP_BCAST, help="Broadcast method - repeating to all known aircraft or sending to IP broadcast address. [default: %(default)s]")
|
||||
parser.add_argument("--broadcast_address", default=DEFAULT_BROADCAST, help="IP address used for broadcast when broadcast method is IP_BCAST. [default: %(default)s]")
|
||||
parser.add_argument("--uplink_port", default=pprzlink.udp.UPLINK_PORT, help="Uplink UDP port. [default: %(default)s]")
|
||||
parser.add_argument("--downlink_port", default=pprzlink.udp.DOWNLINK_PORT, help="Downlink UDP port. [default: %(default)s]")
|
||||
parser.add_argument("--bus", default=pprzlink.ivy.IVY_BUS, help="Ivy bus. [default to system IVY bus]")
|
||||
args = parser.parse_args()
|
||||
|
||||
link = UDPLink(args)
|
||||
link.run()
|
||||
|
||||
@@ -37,9 +37,6 @@
|
||||
#include "pprzlink/pprz_transport.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
/* Message id helpers */
|
||||
#define SenderIdOfPprzMsg(x) (x[0])
|
||||
#define IdOfPprzMsg(x) (x[1])
|
||||
|
||||
#include "nps_main.h"
|
||||
#include "nps_sensors.h"
|
||||
|
||||
Reference in New Issue
Block a user