Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[pprzlink] allow to compile paparazzi with PPRZLINK v2 (#2083)

Browse Source 
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
2017-06-29 10:38:30 +02:00
committed by Michal Podhradsky
parent 2aabe921b3
commit 6a72c5da84
6 changed files with 220 additions and 278 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Makefile
+9 -4
View File
@@ -78,6 +78,11 @@ LOGALIZER=sw/logalizer
SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) sw/tools SUBDIRS = $(PPRZCENTER) $(MISC) $(LOGALIZER) sw/tools
#
# Communication protocol version
#
PPRZLINK_LIB_VERSION ?= 1.0
# #
# xml files used as input for header generation # xml files used as input for header generation
# #
@@ -91,7 +96,7 @@ XSENS_XML = $(CONF)/xsens_MTi-G.xml
# generated header files # generated header files
# #
PPRZLINK_DIR=sw/ext/pprzlink PPRZLINK_DIR=sw/ext/pprzlink
PPRZLINK_INSTALL=$(PAPARAZZI_HOME)/var/lib/ocaml PPRZLINK_INSTALL=$(PAPARAZZI_HOME)/var/lib
MESSAGES_INSTALL=$(PAPARAZZI_HOME)/var MESSAGES_INSTALL=$(PAPARAZZI_HOME)/var
UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h UBX_PROTOCOL_H=$(STATICINCLUDE)/ubx_protocol.h
MTK_PROTOCOL_H=$(STATICINCLUDE)/mtk_protocol.h MTK_PROTOCOL_H=$(STATICINCLUDE)/mtk_protocol.h
@@ -137,7 +142,7 @@ static: cockpit tmtc generators sim_static joystick static_h
libpprzlink: libpprzlink:
$(MAKE) -C $(EXT) pprzlink.update $(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 libpprz: libpprzlink _save_build_version
$(MAKE) -C $(LIB)/ocaml $(MAKE) -C $(LIB)/ocaml
@@ -191,10 +196,10 @@ pprzlink_protocol :
$(Q)test -d $(STATICLIB) || mkdir -p $(STATICLIB) $(Q)test -d $(STATICLIB) || mkdir -p $(STATICLIB)
ifeq ("$(wildcard $(CUSTOM_MESSAGES_XML))","") ifeq ("$(wildcard $(CUSTOM_MESSAGES_XML))","")
@echo GENERATE $@ with default messages @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 else
@echo GENERATE $@ with custome messages from $(CUSTOM_MESSAGES_XML) @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 endif
sw/airborne/subsystems/datalink/datalink.c
+99 -59
View File
@@ -66,81 +66,121 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *
} }
} }
} else { } else {
/* parse telemetry messages coming from ground station */ #if PPRZLINK_DEFAULT_VER == 2
switch (msg_id) { // Check that the message is really a datalink message
case DL_PING: { if (pprzlink_get_msg_class_id(buf) == DL_datalink_CLASS_ID) {
pprz_msg_send_PONG(trans, dev, AC_ID); #endif
} /* parse datalink messages coming from ground station */
break; 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 : { case DL_SETTING : {
if (DL_SETTING_ac_id(buf) != AC_ID) { break; } if (DL_SETTING_ac_id(buf) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(buf); uint8_t i = DL_SETTING_index(buf);
float var = DL_SETTING_value(buf); float var = DL_SETTING_value(buf);
DlSetting(i, var); DlSetting(i, var);
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &var); #if PPRZLINK_DEFAULT_VER == 2
} // Reply to the sender of the message
break; 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 : { case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; } if (DL_GET_SETTING_ac_id(buf) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(buf); uint8_t i = DL_GET_SETTING_index(buf);
float val = settings_get_value(i); float val = settings_get_value(i);
pprz_msg_send_DL_VALUE(trans, dev, AC_ID, &i, &val); #if PPRZLINK_DEFAULT_VER == 2
} // Reply to the sender of the message
break; 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 #ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH : 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) {
#ifdef RADIO_CONTROL_DATALINK_LED #ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif #endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(buf), parse_rc_3ch_datalink(
DL_RC_4CH_throttle(buf), DL_RC_3CH_throttle_mode(buf),
DL_RC_4CH_roll(buf), DL_RC_3CH_roll(buf),
DL_RC_4CH_pitch(buf), DL_RC_3CH_pitch(buf));
DL_RC_4CH_yaw(buf)); break;
} case DL_RC_4CH :
break; 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 #endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS #if USE_GPS
case DL_GPS_INJECT : { case DL_GPS_INJECT : {
// Check if the GPS is for this AC // Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; } if (DL_GPS_INJECT_ac_id(buf) != AC_ID) { break; }
// GPS parse data // GPS parse data
gps_inject_data( gps_inject_data(
DL_GPS_INJECT_packet_id(buf), DL_GPS_INJECT_packet_id(buf),
DL_GPS_INJECT_data_length(buf), DL_GPS_INJECT_data_length(buf),
DL_GPS_INJECT_data(buf) DL_GPS_INJECT_data(buf)
); );
} }
break; break;
#if USE_GPS_UBX_RTCM #if USE_GPS_UBX_RTCM
case DL_RTCM_INJECT : { case DL_RTCM_INJECT : {
// GPS parse data // GPS parse data
gps_inject_data(DL_RTCM_INJECT_packet_id(buf), gps_inject_data(DL_RTCM_INJECT_packet_id(buf),
DL_RTCM_INJECT_data_length(buf), DL_RTCM_INJECT_data_length(buf),
DL_RTCM_INJECT_data(buf)); DL_RTCM_INJECT_data(buf));
} }
break; break;
#endif // USE_GPS_UBX_RTCM #endif // USE_GPS_UBX_RTCM
#endif // USE_GPS #endif // USE_GPS
default: default:
break; break;
}
#if PPRZLINK_DEFAULT_VER == 2
} }
#endif
} }
/* Parse firmware specific datalink */ /* Parse firmware specific datalink */
firmware_parse_msg(dev, trans, buf); firmware_parse_msg(dev, trans, buf);
sw/airborne/subsystems/datalink/datalink.h
-4
View File
@@ -36,10 +36,6 @@
#include "std.h" #include "std.h"
#include "pprzlink/dl_protocol.h" #include "pprzlink/dl_protocol.h"
/* Message id helpers */
#define SenderIdOfPprzMsg(x) (x[0])
#define IdOfPprzMsg(x) (x[1])
/** Datalink kinds */ /** Datalink kinds */
#define PPRZ 1 #define PPRZ 1
#define XBEE 2 #define XBEE 2
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: 2e0309c3e3...42e0492bf1
sw/ground_segment/python/udp_link/udp_link.py
+111 -207
View File
@@ -1,155 +1,147 @@
#!/usr/bin/env python #!/usr/bin/env python
from ivy.std_api import *
import socket
import struct
import os import os
import logging
import sys import sys
import threading
import time
# if PAPARAZZI_SRC not set, then assume the tree containing this # if PAPARAZZI_SRC not set, then assume the tree containing this
# file is a reasonable substitute # file is a reasonable substitute
PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), 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")
sys.path.append(PPRZ_SRC + "/sw/lib/python")
sys.path.append(PPRZ_SRC + "/sw/ext/pprzlink/lib/v1.0/python")
import pprz_env from ivy.std_api import *
from pprzlink import messages_xml_map 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 PING_PERIOD = 5.0
STATUS_PERIOD = 1.0 STATUS_PERIOD = 1.0
STX = 0x99
STX_TS = 0x98
DATALINK_PORT = 4243
DOWNLINK_PORT = 4242
class DownLinkStatus(): class DownLinkStatus():
def __init__(self, ac_id, address): def __init__(self, ac_id, address):
self.ac_id = ac_id self.ac_id = ac_id
self.address = address self.address = address
self.rx_bytes = 0 self.rx_bytes = 0
self.rx_msgs = 0 self.rx_msgs = 0
self.tx_msgs = 0
self.run_time = 0 self.run_time = 0
self.last_rx_bytes = 0 self.last_rx_bytes = 0
self.last_rx_msgs = 0 self.last_rx_msgs = 0
self.last_msg_time = 0
self.last_ping_time = 0 self.last_ping_time = 0
self.last_pong_time = 0 self.last_pong_time = 0
class UDPLink:
class IvyUdpLink(): def __init__(self,opts):
def __init__(self): messages_xml_map.parse_messages()
self.InitIvy() self.run_event = threading.Event()
self.ivy_id = 0 self.uplink_port = opts.uplink_port
self.status_timer = threading.Timer(STATUS_PERIOD, self.sendStatus) self.downlink_port = opts.downlink_port
self.ping_timer = threading.Timer(STATUS_PERIOD, self.sendPing) 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.ac_downlink_status = {}
self.rx_err = 0 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() def updateStatus(self, ac_id, length, address, isPong):
self.data_types = {'float': ['f', 4], if not self.ac_downlink_status.has_key(ac_id):
'uint8': ['B', 1], self.ac_downlink_status[ac_id] = DownLinkStatus(ac_id, address)
'uint16': ['H', 2], self.ac_downlink_status[ac_id].rx_msgs += 1
'uint32': ['L', 4], self.ac_downlink_status[ac_id].rx_bytes += length
'int8': ['b', 1], self.ac_downlink_status[ac_id].last_msg_time = time.time()
'int16': ['h', 2], if isPong:
'int32': ['l', 4] self.ac_downlink_status[ac_id].last_pong_time = time.time() - self.ac_downlink_status[ac_id].last_ping_time
}
def __del__(self): def proccess_downlink_msg(self,sender,address,msg,length,receiver_id=None, component_id=None):
self.stop() 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): def proccess_uplink_msg(self,ac_id,msg):
self.status_timer.cancel() # print ('New IVY message to %s : %s' % (ac_id,msg))
self.ping_timer.cancel() if msg.broadcasted:
IvyUnBindMsg(self.ivy_id) if self.bcast_method==IP_BCAST:
IvyStop() self.udp.send(msg,0,(self.bcast_addr,self.uplink_port))
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]))
else: else:
typed_args.append(int(args[idx])) for dest in self.ac_downlink_status.keys():
idx += 1 self.udp.send(msg, 0, (self.ac_downlink_status[dest].address[0], self.uplink_port))
msg = struct.pack(struct_string, stx, length, sender, msg_id, *typed_args) self.ac_downlink_status[dest].tx_msgs += 1
(ck_a, ck_b) = self.calculate_checksum(msg) else:
msg = msg + struct.pack('=BB', ck_a, ck_b) if isinstance(ac_id,str):
return msg 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): def initial_ivy_binds(self):
list = larg[0].split(' ') # Subscribe to all datalink messages
sender = list[0] messages_datalink = messages_xml_map.get_msgs("datalink")
msg_name = list[1] for msg in messages_datalink:
ac_id = list[3] self.ivy.subscribe(self.proccess_uplink_msg, message.PprzMessage("datalink", msg))
args = list[2:]
msg_id = messages_xml_map.message_dictionary_name_id["datalink"][msg_name] def run(self):
if self.ac_downlink_status.has_key(int(ac_id)): print ('Starting UDPLink for protocol version %s' % (messages_xml_map.PROTOCOL_VERSION))
msgbuf = self.buildPprzMsg(msg_id, *args) self.udp.start()
address = (self.ac_downlink_status[int(ac_id)].address[0], DATALINK_PORT) self.ivy.start()
self.server.sendto(msgbuf, address)
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): def sendPing(self):
for (ac_id, value) in self.ac_downlink_status.items(): for (ac_id, value) in self.ac_downlink_status.items():
msg_id = messages_xml_map.message_dictionary_name_id["datalink"]["PING"] if messages_xml_map.PROTOCOL_VERSION=="2.0":
msgbuf = self.buildPprzMsg(msg_id) # For pprzlink V2.0 set the receiver id
address = (self.ac_downlink_status[int(ac_id)].address[0], DATALINK_PORT) self.udp.send(message.PprzMessage('datalink','PING'), 0, self.ac_downlink_status[int(ac_id)].address, ac_id)
self.server.sendto(msgbuf, address) else:
value.last_ping_time = time.clock() 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(STATUS_PERIOD, self.sendPing) self.ping_timer = threading.Timer(PING_PERIOD, self.sendPing)
self.ping_timer.start() self.ping_timer.start()
def sendStatus(self): def sendStatus(self):
for (key, value) in self.ac_downlink_status.items(): 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, value.ac_id,
-1,
value.run_time, value.run_time,
time.time() - value.last_msg_time,
value.rx_bytes, value.rx_bytes,
value.rx_msgs, value.rx_msgs,
self.rx_err, self.rx_err,
value.rx_bytes - value.last_rx_bytes, value.rx_bytes - value.last_rx_bytes,
value.rx_msgs - value.last_rx_msgs, value.rx_msgs - value.last_rx_msgs,
value.tx_msgs,
1000 * value.last_pong_time)) 1000 * value.last_pong_time))
value.last_rx_bytes = value.rx_bytes value.last_rx_bytes = value.rx_bytes
value.last_rx_msgs = value.rx_msgs 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 = threading.Timer(STATUS_PERIOD, self.sendStatus)
self.status_timer.start() 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__': 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()
sw/simulator/nps/nps_main_hitl.c
-3
View File
@@ -37,9 +37,6 @@
#include "pprzlink/pprz_transport.h" #include "pprzlink/pprz_transport.h"
#include "generated/airframe.h" #include "generated/airframe.h"
/* Message id helpers */
#define SenderIdOfPprzMsg(x) (x[0])
#define IdOfPprzMsg(x) (x[1])
#include "nps_main.h" #include "nps_main.h"
#include "nps_sensors.h" #include "nps_sensors.h"