def forward_heartbeat(self, msg_class, msg_name, sender, message):
hb = HeartBeat()
hb.ParseFromString(message)
# TELEMETRY_STATUS message
msg = PprzMessage("ground", "TELEMETRY_STATUS")
values = [
str(self.aircraft_id), "no_id", "0.02", "0", "0", "0.0", "0", "0",
"0", "0", "0", "9999.0"
]
msg.set_values(values)
self.interface.send(msg)
def on_ivy_msg(self, agent, *larg):
""" Split ivy message up into the separate parts
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
"""
# return if no callback is set
if self.callback is None:
return
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', larg[0])
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# check which message class it is
# pass non-telemetry messages with ac_id 0
if data[0] in ["sim", "ground_dl", "dl"]:
if self.verbose:
print("ignoring message " + larg[0])
sys.stdout.flush()
return
elif data[0] in ["ground"]:
msg_class = data[0]
msg_name = data[1]
ac_id = 0
values = list(filter(None, data[2:]))
else:
try:
ac_id = int(data[0])
except ValueError:
if self.verbose:
print("ignoring message " + larg[0])
sys.stdout.flush()
return
msg_class = "telemetry"
msg_name = data[1]
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def on_ivy_msg(self, agent, *larg):
""" Split ivy message up into the separate parts
Basically parts/args in string are separated by space, but char array can also contain a space:
|f,o,o, ,b,a,r| in old format or "foo bar" in new format
"""
# return if no callback is set
if self.callback is None:
return
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', larg[0])
# strip spaces and filter out emtpy strings
l = [str.strip(s) for s in l if str.strip(s) is not '']
data = []
for s in l:
# split non-array strings further up
if '|' not in s and '"' not in s:
data += s.split(' ')
else:
data.append(s)
# ignore ivy message with less than 3 elements
if len(data) < 3:
return
# check which message class it is
# pass non-telemetry messages with ac_id 0
if data[0] in ["sim", "ground_dl", "dl"]:
if self.verbose:
print("ignoring message " + larg[0])
sys.stdout.flush()
return
elif data[0] in ["ground"]:
msg_class = data[0]
msg_name = data[1]
ac_id = 0
values = list(filter(None, data[2:]))
else:
try:
ac_id = int(data[0])
except ValueError:
if self.verbose:
print("ignoring message " + larg[0])
sys.stdout.flush()
return
msg_class = "telemetry"
msg_name = data[1]
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def forward_position(self, msg_class, msg_name, sender, message):
pos = Position()
pos.ParseFromString(message)
# FLIGHT_PARAM message
msg = PprzMessage("ground", "FLIGHT_PARAM")
values = [
str(self.aircraft_id), "0.0", "0.0",
str(pos.hdg),
str(pos.lat),
str(pos.lon),
str(pos.vground), "0.0",
str(pos.alt),
str(pos.vz),
str(pos.relalt), "0.0", "0", "0.0"
]
msg.set_values(values)
self.interface.send(msg)
def move_waypoint(self, ac_id, wp_id, lat, lon, alt):
msg = PprzMessage("ground", "MOVE_WAYPOINT")
msg['ac_id'] = ac_id
msg['wp_id'] = wp_id
msg['lat'] = lat
msg['long'] = lon
msg['alt'] = alt
print("Sending message: %s" % msg)
self._interface.send(msg)
def codeRecognized (self, data):
print data
if data == '1':
coords = 1
print "Dropzone 1"
elif data == '2':
coords = 2
print "Dropzone 2"
elif data == '3':
coords = 3
print "Dropzone 3"
else:
print data
return
print('Going to: ', coords)
pprzmsg = PprzMessage("ground", "DL_SETTING")
pprzmsg.set_values([self.ac_id, self.waypoint_id, coords])
self.interface.send(pprzmsg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def send_raw_datalink(self, msg):
if not isinstance(msg, PprzMessage):
print("Can only send PprzMessage")
return
if "datalink" not in msg.msg_class:
print(
"Message to embed in RAW_DATALINK needs to be of 'datalink' class"
)
return
raw = PprzMessage("ground", "RAW_DATALINK")
raw['ac_id'] = msg['ac_id']
raw['message'] = msg.to_csv()
self.send(raw)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x01
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x03
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def test():
import time
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("-f", "--file", help="path to messages.xml file")
parser.add_argument("-c",
"--class",
help="message class",
dest='msg_class',
default='telemetry')
parser.add_argument("-d",
"--device",
help="device name",
dest='dev',
default='/dev/ttyUSB0')
parser.add_argument("-b",
"--baudrate",
help="baudrate",
dest='baud',
default=115200,
type=int)
args = parser.parse_args()
pprz_msg.messages_xml_map.parse_messages(args.file)
serial_interface = SerialMessagesInterface(
lambda s, m: print("new message from %i: %s" % (s, m)),
device=args.dev,
baudrate=args.baud,
msg_class=args.msg_class,
verbose=True)
print("Starting serial interface on %s at %i baud" % (args.dev, args.baud))
try:
serial_interface.start()
# give the thread some time to properly start
time.sleep(0.1)
# send some datalink messages to aicraft for test
ac_id = 42
print("sending ping")
ping = PprzMessage('datalink', 'PING')
serial_interface.send(ping, 0)
get_setting = PprzMessage('datalink', 'GET_SETTING')
get_setting['index'] = 0
get_setting['ac_id'] = ac_id
serial_interface.send(get_setting, 0)
# change setting with index 0 (usually the telemetry mode)
set_setting = PprzMessage('datalink', 'SETTING')
set_setting['index'] = 0
set_setting['ac_id'] = ac_id
set_setting['value'] = 1
serial_interface.send(set_setting, 0)
# block = PprzMessage('datalink', 'BLOCK')
# block['block_id'] = 3
# block['ac_id'] = ac_id
# serial_interface.send(block, 0)
while serial_interface.isAlive():
serial_interface.join(1)
except (KeyboardInterrupt, SystemExit):
print('Shutting down...')
serial_interface.stop()
exit()