def test():
'''
run test program as a module to avoid namespace conflicts with serial module:
python -p pprzlink.serial
pprzlink should be installed in a python standard path or included to your PYTHONPATH
'''
import time
import argparse
from pprzlink import messages_xml_map
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)
parser.add_argument("-id", "--ac_id", help="aircraft id (receiver)", dest='ac_id', default=42, type=int)
parser.add_argument("--interface_id", help="interface id (sender)", dest='id', default=0, type=int)
args = parser.parse_args()
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, interface_id=args.id, 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 = args.ac_id
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()
def callback_depth(depth):
image_buffer = drone.bridge.imgmsg_to_cv2(depth,
desired_encoding="passthrough")
mean_left = 0
mean_right = 0
matrix = depth.height * depth.width
for row in range(depth.height):
for col in range(depth.width):
pix = image_buffer[row][col]
if pix < 1000:
if (col < depth.width / 2):
mean_left += pix
else:
mean_right += pix
mean_left /= matrix * 0.5
mean_right /= matrix * 0.5
message = PprzMessage("intermcu", "IMCU_LR_MEAN_DIST")
message.set_values([np.float(mean_left), np.float(mean_right)])
serial.interface.send(message, drone.id)
def test():
import time
import argparse
from pprzlink import messages_xml_map
parser = argparse.ArgumentParser()
parser.add_argument("-f", "--file", help="path to messages.xml file")
parser.add_argument("-c", "--class", help="message class of incoming messages", dest='msg_class', default='telemetry')
parser.add_argument("-a", "--address", help="destination address", dest='address', default='127.0.0.1')
parser.add_argument("-id", "--ac_id", help="aircraft id (receiver)", dest='ac_id', default=42, type=int)
parser.add_argument("--interface_id", help="interface id (sender)", dest='id', default=0, type=int)
parser.add_argument("-up", "--uplink_port", help="uplink port", dest='uplink', default=UPLINK_PORT, type=int)
parser.add_argument("-dp", "--downlink_port", help="downlink port", dest='downlink', default=DOWNLINK_PORT, type=int)
args = parser.parse_args()
messages_xml_map.parse_messages(args.file)
udp_interface = UdpMessagesInterface(lambda s, a, m: print("new message from %i (%s): %s" % (s, a, m)),
uplink_port=args.uplink, downlink_port=args.downlink,
msg_class=args.msg_class, interface_id=args.id, verbose=True)
print("Starting UDP interface with '%s' with id '%d'" % (args.address, args.ac_id))
address = (args.address, args.uplink)
try:
udp_interface.start()
# give the thread some time to properly start
time.sleep(0.1)
# send some datalink messages to aicraft for test
ac_id = args.ac_id
print("sending ping")
ping = PprzMessage('datalink', 'PING')
udp_interface.send(ping, 0, address)
print("sending get_setting")
get_setting = PprzMessage('datalink', 'GET_SETTING')
get_setting['index'] = 0
get_setting['ac_id'] = ac_id
udp_interface.send(get_setting, 0, address)
# change setting with index 0 (usually the telemetry mode)
print("sending setting")
set_setting = PprzMessage('datalink', 'SETTING')
set_setting['index'] = 0
set_setting['ac_id'] = ac_id
set_setting['value'] = 1
udp_interface.send(set_setting, 0, address)
# print("sending block")
# block = PprzMessage('datalink', 'BLOCK')
# block['block_id'] = 3
# block['ac_id'] = ac_id
# udp_interface.send(block, 0, address)
while udp_interface.isAlive():
udp_interface.join(1)
except (KeyboardInterrupt, SystemExit):
print('Shutting down...')
udp_interface.stop()
exit()
def circular_formation(self):
'''
circular formation control algorithm
'''
ready = True
for ac in self.aircraft:
if (not ac.initialized_nav) or (not ac.initialized_gvf):
if self.verbose:
print("Waiting for state of aircraft ", ac.id)
ready = False
if not ready:
return
i = 0
for ac in self.aircraft:
ac.sigma = np.arctan2(ac.XY[1] - ac.XYc[1], ac.XY[0] - ac.XYc[0])
self.sigmas[i] = ac.sigma
i = i + 1
inter_sigma = self.B.transpose().dot(self.sigmas)
error_sigma = inter_sigma - self.Zdesired
if np.size(error_sigma) > 1:
for i in range(0, np.size(error_sigma)):
if error_sigma[i] > np.pi:
error_sigma[i] = error_sigma[i] - 2 * np.pi
elif error_sigma[i] <= -np.pi:
error_sigma[i] = error_sigma[i] + 2 * np.pi
else:
if error_sigma > np.pi:
error_sigma = error_sigma - 2 * np.pi
elif error_sigma <= -np.pi:
error_sigma = error_sigma + 2 * np.pi
u = -self.aircraft[0].s * self.k * self.B.dot(error_sigma)
if self.verbose:
print(
"Inter-vehicle errors: ",
str(error_sigma * 180.0 / np.pi).replace('[',
'').replace(']', ''))
i = 0
for ac in self.aircraft:
msga = PprzMessage("ground", "DL_SETTING")
msga['ac_id'] = ac.id
msga['index'] = ac.a_index
msga['value'] = self.radius + u[i]
msgb = PprzMessage("ground", "DL_SETTING")
msgb['ac_id'] = ac.id
msgb['index'] = ac.b_index
msgb['value'] = self.radius + u[i]
self._interface.send(msga)
self._interface.send(msgb)
i = i + 1
def __init__(self, ac_id, plateform_id, tag_id):
# Start a ivy messages listener named PIR DRONE
self._interface = IvyMessagesInterface("PIR", ivy_bus="192.168.1:2010")
self.drone = FC_Rotorcraft(ac_id, tag_id, self._interface)
self.plateform = Platform(plateform_id)
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id == self.drone.id:
# X and V in NED
self.drone.X_opti[0] = float(msg['pos'][1])
self.drone.X_opti[1] = float(msg['pos'][0])
self.drone.X_opti[2] = -float(msg['pos'][2])
self.drone.V_opti[0] = float(msg['speed'][1])
self.drone.V_opti[1] = float(msg['speed'][0])
self.drone.V_opti[2] = -float(msg['speed'][2])
self.drone.quaternions[0] = float(msg['quat'][0])
self.drone.quaternions[1] = float(msg['quat'][1])
self.drone.quaternions[2] = float(msg['quat'][2])
self.drone.quaternions[3] = float(msg['quat'][3])
self.drone.timeLastUpdate = time.time()
self.drone.initialized = True
if ac_id == self.plateform.id:
# X and V in NED
self.plateform.X[0] = float(msg['pos'][1])
self.plateform.X[1] = float(msg['pos'][0])
self.plateform.X[2] = -float(msg['pos'][2])
self.plateform.V[0] = float(msg['speed'][1])
self.plateform.V[1] = float(msg['speed'][0])
self.plateform.V[2] = -float(msg['speed'][2])
self.plateform.quaternions[0] = float(msg['quat'][0])
self.plateform.quaternions[1] = float(msg['quat'][1])
self.plateform.quaternions[2] = float(msg['quat'][2])
self.plateform.quaternions[3] = float(msg['quat'][3])
self.plateform.timeLastUpdate = time.time()
self.plateform.initialized = True
self._interface.subscribe(ground_ref_cb,
PprzMessage("ground", "GROUND_REF"))
# Recuperation du x et y via l'UWB
def get_xy_uwb(uwb_id, msg):
# X and V in NED
#msg est un tableau contenant : d1corr, d2corr, d3corr, d1vrai, d2vrai, d3vrai, x, y, z, vx, vy, vz
x = float(msg['values'][6])
y = float(msg['values'][7])
z = self.drone.X_opti[3]
psi = cr.getEulerAngles(
self.drone.quaternions)[2] - cr.getEulerAngles(
self.plateform.quaternions)[2]
#self.drone.V_uwb[0] = float(msg['values'][9])
#self.drone.V_uwb[1] = float(msg['values'][10])
self.drone.X_uwb = cr.uwb2pseudoBody(x, y, z)
self._interface.subscribe(get_xy_uwb,
PprzMessage("telemetry", "PAYLOAD_FLOAT"))
def formation(B, ds, radius, k):
waiting_for_msgs = 0
for ac in list_aircraft:
if ac.a == -999:
print("Waiting for GVF msg of aircraft ", ac.id)
waiting_for_msgs = 1
if ac.XY[0] == -999:
print("Waiting for NAV msg of aircraft ", ac.id)
waiting_for_msgs = 1
if waiting_for_msgs == 1:
return
sigma = np.zeros(len(list_aircraft))
i = 0
for ac in list_aircraft:
ac.sigma = np.arctan2(ac.XY[1]-ac.XYc[1], ac.XY[0]-ac.XYc[0])
sigma[i] = ac.sigma
i = i + 1
inter_sigma = B.transpose().dot(sigma)
error_sigma = inter_sigma - ds
if np.size(error_sigma) > 1:
for i in range(0, np.size(error_sigma)):
if error_sigma[i] > np.pi:
error_sigma[i] = error_sigma[i] - 2*np.pi
elif error_sigma[i] <= -np.pi:
error_sigma[i] = error_sigma[i] + 2*np.pi
else:
if error_sigma > np.pi:
error_sigma = error_sigma - 2*np.pi
elif error_sigma <= -np.pi:
error_sigma = error_sigma + 2*np.pi
u = -list_aircraft[0].s*k*B.dot(error_sigma)
print(list_aircraft[0].time, " ", str(error_sigma*180.0/np.pi).replace('[','').replace(']',''))
i = 0
for ac in list_aircraft:
msga = PprzMessage("ground", "DL_SETTING")
msga['ac_id'] = ac.id
msga['index'] = ac.a_index
msga['value'] = radius + u[i]
msgb = PprzMessage("ground", "DL_SETTING")
msgb['ac_id'] = ac.id
msgb['index'] = ac.b_index
msgb['value'] = radius + u[i]
interface.send(msga)
interface.send(msgb)
i = i + 1
return
def __init__(self, config, freq=10., verbose=False):
self.config = config
self.step = 1. / freq
self.verbose = verbose
self.ids = self.config['ids']
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.k = np.array(self.config['gain'])
self.radius = np.array(self.config['desired_stationary_radius'])
self.aircraft = [Aircraft(i) for i in self.ids]
self.sigmas = np.zeros(len(self.aircraft))
for ac in self.aircraft:
settings = PaparazziACSettings(ac.id)
list_of_indexes = ['ell_a', 'ell_b']
for setting_ in list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'ell_a':
ac.a_index = index
if setting_ == 'ell_b':
ac.b_index = index
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten to check gvf.xml for your settings?"
)
# Start IVY interface
self._interface = IvyMessagesInterface("Circular Formation")
# bind to NAVIGATION message
def nav_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "NAVIGATION":
ac = self.aircraft[self.ids.index(ac_id)]
ac.XY[0] = float(msg.get_field(2))
ac.XY[1] = float(msg.get_field(3))
ac.initialized_nav = True
self._interface.subscribe(nav_cb, PprzMessage("telemetry",
"NAVIGATION"))
def gvf_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "GVF":
if int(msg.get_field(1)) == 1:
ac = self.aircraft[self.ids.index(ac_id)]
param = msg.get_field(4)
ac.XYc[0] = float(param[0])
ac.XYc[1] = float(param[1])
ac.a = float(param[2])
ac.b = float(param[3])
ac.s = float(msg.get_field(2))
ac.initialized_gvf = True
self._interface.subscribe(gvf_cb, PprzMessage("telemetry", "GVF"))
def unpack_pprz_msg(self, data):
"""Unpack a raw PPRZ message"""
sender_id = data[0]
receiver_id = data[1]
class_id = data[2] & 0x0F
component_id = (data[2] & 0xF0) >> 4
msg_id = data[3]
msg = PprzMessage(class_id, msg_id)
msg.binary_to_payload(data[4:])
return sender_id, receiver_id, component_id, msg
def parse_pprz_msg(callback, ivy_msg):
"""
Parse an Ivy message into a PprzMessage.
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
:param callback: function to call with ac_id and parsed PprzMessage as params
:param ivy_msg: Ivy message string to parse into PprzMessage
"""
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
# 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
# normal format is "sender_name msg_name msg_payload..."
# advanced format has requests and answers (with request_id as 'pid_index')
# request: "sender_name request_id msg_name_REQ msg_payload..."
# answer: "request_id sender_name msg_name msg_payload..."
# check for request_id in first or second string (-> advanced format with msg_name in third string)
advanced = False
if re.search("[0-9]+_[0-9]+", data[0]) or re.search(
"[0-9]+_[0-9]+", data[1]):
advanced = True
msg_name = data[2] if advanced else data[1]
# check which message class it is
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + ivy_msg)
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + ivy_msg)
sys.stdout.flush()
else:
ac_id = 0
payload = data[3:] if advanced else data[2:]
values = list(filter(None, payload))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
# finally call the callback, passing the aircraft id and parsed message
callback(ac_id, msg)
def parse_pprz_msg(callback, ivy_msg):
"""
Parse an Ivy message into a PprzMessage.
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
:param callback: function to call with ac_id and parsed PprzMessage as params
:param ivy_msg: Ivy message string to parse into PprzMessage
"""
# first split on array delimiters
l = re.split('([|\"][^|\"]*[|\"])', ivy_msg)
# 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
# normal format is "sender_name msg_name msg_payload..."
# advanced format has requests and answers (with request_id as 'pid_index')
# request: "sender_name request_id msg_name_REQ msg_payload..."
# answer: "request_id sender_name msg_name msg_payload..."
# check for request_id in first or second string (-> advanced format with msg_name in third string)
advanced = False
if re.search("[0-9]+_[0-9]+", data[0]) or re.search("[0-9]+_[0-9]+", data[1]):
advanced = True
msg_name = data[2] if advanced else data[1]
# check which message class it is
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + ivy_msg)
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + ivy_msg)
sys.stdout.flush()
else:
ac_id = 0
payload = data[3:] if advanced else data[2:]
values = list(filter(None, payload))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
# finally call the callback, passing the aircraft id and parsed message
callback(ac_id, msg)
def parse_pprz_msg(callback, ivy_msg):
"""
Parse an Ivy message into a PprzMessage.
:param callback: function to call with ac_id and parsed PprzMessage as params
:param ivy_msg: Ivy message string to parse into PprzMessage
"""
# normal format is "sender_name msg_name msg_payload..."
# advanced format has requests and answers (with request_id as 'pid_index')
# request: "sender_name request_id msg_name_REQ msg_payload..."
# answer: "request_id sender_name msg_name msg_payload..."
data = re.search("(\S+) +(\S+) +(.*)", ivy_msg)
# check for request_id in first or second string (-> advanced format with msg_name in third string)
if data is None:
return
if re.search("[0-9]+_[0-9]+", data.group(1)) or re.search("[0-9]+_[0-9]+", data.group(2)):
if re.search("[0-9]+_[0-9]+", data.group(1)):
sender_name = data.group(2)
else:
sender_name = data.group(1)
# this is an advanced type, split again
data = re.search("(\S+) +(.*)", data.group(3))
msg_name = data.group(1)
payload = data.group(2)
else:
# this was a normal message
sender_name = data.group(1)
msg_name = data.group(2)
payload = data.group(3)
# check which message class it is
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + ivy_msg)
return
msg = PprzMessage(msg_class, msg_name)
msg.ivy_string_to_payload(payload)
# pass non-telemetry messages with ac_id 0 or ac_id attrib value
if msg_class == "telemetry":
try:
if(sender_name[0:6] == 'replay'):
ac_id = int(sender_name[6:])
else:
ac_id = int(sender_name)
except ValueError:
print("ignoring message " + ivy_msg)
sys.stdout.flush()
else:
if 'ac_id' in msg.fieldnames:
ac_id_idx = msg.fieldnames.index('ac_id')
ac_id = msg.fieldvalues[ac_id_idx]
else:
ac_id = 0
# finally call the callback, passing the aircraft id and parsed message
callback(ac_id, msg)
def init_dcftables(self):
time.sleep(2)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
# nei_id = 0, special msg to clean the table onboard
msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
msg_clean_a['nei_id'] = 0
msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
msg_clean_b['nei_id'] = 0
self._interface.send(msg_clean_a)
self._interface.send(msg_clean_b)
if self.verbose:
print(msg_clean_a)
print(msg_clean_b)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
msga = PprzMessage("datalink", "DCF_REG_TABLE")
msga['ac_id'] = int(self.list_ids[i[0]])
msga['nei_id'] = int(self.list_ids[i[1]])
if len(self.list_ids) == 2:
msga['desired_sigma'] = (column[index])[0] * int(self.Zdesired)
else:
msga['desired_sigma'] = (column[index])[0] * int(
self.Zdesired[count])
self._interface.send(msga)
msgb = PprzMessage("datalink", "DCF_REG_TABLE")
msgb['ac_id'] = int(self.list_ids[i[1]])
msgb['nei_id'] = int(self.list_ids[i[0]])
if len(self.list_ids) == 2:
msgb['desired_sigma'] = (column[index])[1] * int(self.Zdesired)
else:
msgb['desired_sigma'] = (column[index])[1] * int(
self.Zdesired[count])
self._interface.send(msgb)
if self.verbose:
print(msga)
print(msgb)
time.sleep(2)
def __init__(self, verbose=False):
# init cam related part
uEyePprzlink.__init__(self, verbose)
# build fake message
msg = PprzMessage("telemetry", "DC_SHOT")
msg['photo_nr'] = 0
self.process_msg(0, msg)
def mission_segment(self,
s_pos,
d_pos,
altitude,
duration=0,
insert_mode=0):
"""Send a MISSION_SEGMENT command to paparazzi.
Arguments:
s_pos: source position as tuple (east, north)
d_pos: destination position as tuple (east, north)
altitude: altitude of the segment (in meters above sea level)
duration: duration of mission (in seconds) before being discarded.
Default is 0, meaning no specified duration.
insert_mode: default APPEND
0: APPEND
1: PREPEND
2: REPLACE_CURRENT
3: REPLACE_ALL
"""
msg = PprzMessage("datalink", "MISSION_SEGMENT")
msg['ac_id'] = self.ac_id
# APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
msg['insert'] = insert_mode
msg['segment_east_1'] = s_pos[0]
msg['segment_north_1'] = s_pos[1]
msg['segment_east_2'] = d_pos[0]
msg['segment_north_2'] = d_pos[1]
msg['segment_alt'] = altitude
msg['duration'] = duration
self.ivy_mes.send_raw_datalink(msg)
def receiveRigidBodyList(rigidBodyList, stamp):
for (ac_id, pos, quat) in rigidBodyList:
i = str(ac_id)
if i not in id_dict.keys():
continue
store_track(i, pos, stamp)
if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
if timestamp[i] is None:
timestamp[i] = stamp
continue # too early for next message
timestamp[i] = stamp
msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
msg['ac_id'] = id_dict[i]
msg['pad'] = 0
msg['enu_x'] = pos[0]
msg['enu_y'] = pos[1]
msg['enu_z'] = pos[2]
vel = compute_velocity(i)
msg['enu_xd'] = vel[0]
msg['enu_yd'] = vel[1]
msg['enu_zd'] = vel[2]
msg['tow'] = int(stamp) # TODO convert to GPS itow ?
# convert quaternion to psi euler angle
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
ivy.send(msg)
def send_pprz_fault_info(self):
if self.verbose: print('Sending the FAULT_INFO')
set_fault = PprzMessage('datalink', 'FAULT_INFO')
set_fault['info'] = self.fault_info
set_fault['type'] = self.fault_type
set_fault['class'] = self.fault_class
self.interface.send(set_fault, 0)
def guidance_setmode(ac_id, value):
retval = ''
try:
settings = PaparazziACSettings(ac_id)
except Exception as e:
print(e)
return
try:
index = settings.name_lookup['auto2'].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
return
if index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = ac_id
msg['index'] = index
msg['value'] = value
if verbose:
print_ivy_trace(msg)
retval = 'Guidance mode: ac_id=%d, index=%d, value=%d\n' % (
ac_id, index, value)
ivy_interface.send(msg)
if curl: print_curl_format()
return retval
def __init__(self, my_ac_id, tag_id, interface_):
# Start a ivy messages listener named PIR DRONE
self._interface = interface_
self.tag = ArUcoTag(tag_id)
self.ac_id = my_ac_id
# bind to jevois message
def jevois_cb(jevois_id, msg):
# print("entree dans callback : {} {}".format(self.tag.id , str(msg['id'])))
if self.tag.id == str(msg['id']).replace('"', ""):
self.tag.X[0] = float(
msg['coord']
[0]) / 1000 # data is sent in milimeter , stocked in meter
self.tag.X[1] = float(msg['coord'][1]) / 1000
self.tag.X[2] = float(msg['coord'][2]) / 1000
self.tag.quaternions[0] = float(msg['quat'][0])
self.tag.quaternions[1] = float(msg['quat'][1])
self.tag.quaternions[2] = float(msg['quat'][2])
self.tag.quaternions[3] = float(msg['quat'][3])
self.tag.whd[0] = float(msg['dim'][0])
self.tag.whd[1] = float(msg['dim'][1])
self.tag.whd[2] = float(msg['dim'][2])
self.tag.initialized = True
self.tag.timeLastUpdate = time.time()
self._interface.subscribe(jevois_cb,
PprzMessage("telemetry", "JEVOIS"))
def __setitem__(self, key, value):
setting = self.settings_grp[key]
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = setting.index
msg['value'] = value
self.ivy.send(msg)
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def mission_goto_wp(self, east, north, up, duration=0, insert_mode=0):
"""Send a MISSION_GOTO_WP command to paparazzi.
Arguments:
east: position east
north: position north
up: position up (in meters above sea level)
duration: duration of mission before being discarded. Default is 0,
meaning no specified duration.
insert_mode: default APPEND
0: APPEND
1: PREPEND
2: REPLACE_CURRENT
3: REPLACE_ALL
"""
msg = PprzMessage("datalink", "MISSION_GOTO_WP")
msg['ac_id'] = self.ac_id
# APPEND, PREPEND, REPLACE_CURRENT, REPLACE_ALL
msg['insert'] = insert_mode
# ENU to NED coordinates
msg['wp_east'] = east
msg['wp_north'] = north
msg['wp_alt'] = up
msg['duration'] = duration
self.ivy_mes.send_raw_datalink(msg)
def _message_req(self, msg_name, cb, params=None):
bind_id = None
def _cb(sender, msg):
if bind_id is not None:
self._ivy.unsubscribe(bind_id)
cb(sender, msg)
req_id = self._get_req_id()
req_regex = '^{} ([^ ]* +{}( .*|$))'.format(req_id, msg_name)
bind_id = self._ivy.subscribe(_cb, req_regex)
req_msg = PprzMessage('ground', '{}_REQ'.format(msg_name))
if params is not None:
req_msg.set_values(params)
#FIXME we shouldn't use directly Ivy, but pprzlink python API is not supporting the request id for now
IvySendMsg('pprz_connect {} {} {}'.format(
req_id, req_msg.name, req_msg.payload_to_ivy_string()))
def receiveRigidBodyList( rigidBodyList, stamp ):
for (ac_id, pos, quat, valid) in rigidBodyList:
if not valid:
# skip if rigid body is not valid
continue
i = str(ac_id)
if i not in id_dict.keys():
continue
store_track(i, pos, stamp)
if timestamp[i] is None or abs(stamp - timestamp[i]) < period:
if timestamp[i] is None:
timestamp[i] = stamp
continue # too early for next message
timestamp[i] = stamp
msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
msg['ac_id'] = id_dict[i]
msg['pad'] = 0
msg['enu_x'] = pos[0]
msg['enu_y'] = pos[1]
msg['enu_z'] = pos[2]
vel = compute_velocity(i)
msg['enu_xd'] = vel[0]
msg['enu_yd'] = vel[1]
msg['enu_zd'] = vel[2]
msg['tow'] = int(1000. * stamp) # TODO convert to GPS itow ?
# convert quaternion to psi euler angle
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
ivy.send(msg)
# send GROUND_REF message if needed
if args.ground_ref:
gr = PprzMessage("ground", "GROUND_REF")
gr['ac_id'] = str(id_dict[i])
gr['frame'] = "LTP_ENU"
gr['pos'] = pos
gr['speed'] = vel
gr['quat'] = [quat[3], quat[0], quat[1], quat[2]]
gr['rate'] = [ 0., 0., 0. ]
gr['timestamp'] = stamp
ivy.send(gr)
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 send_accelerations(self):
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = self.id
msg['flag'] = 0 # full 3D not supported yet
# Receiving position in ENU, but sending in NED:
msg['uy'] = self.U[0][0]
msg['ux'] = self.U[1][0]
msg['uz'] = 20 #self.U[2][0] # Sending altitude, not acceleration
self._interface.send(msg)
def __init__(self, verbose=False):
from pprzlink.ivy import IvyMessagesInterface
# init Ivy interface
self.pprzivy = IvyMessagesInterface("pyueye")
# init cam related part
uEyePprzlink.__init__(self, verbose)
# bind to message
self.pprzivy.subscribe(self.process_msg,
PprzMessage("telemetry", "DC_SHOT"))
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
msg_name = data[1]
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + larg[0])
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + larg[0])
sys.stdout.flush()
else:
ac_id = 0
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def get_parameter_list(missionType):
if missionType not in MissionBuilder.missionMessagesNames:
raise ValueError(missionType + " is not a MISSION")
msg = PprzMessage('datalink', missionType)
params = []
for field in msg.fieldnames:
# not outputing params common to all missions
if field not in MissionBuilder.commonParameters:
params.append(field)
return params
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
msg_name = data[1]
msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name)
if msg_class is None:
print("Ignoring unknown message " + larg[0])
return
# pass non-telemetry messages with ac_id 0
if msg_class == "telemetry":
try:
ac_id = int(data[0])
except ValueError:
print("ignoring message " + larg[0])
sys.stdout.flush()
else:
ac_id = 0
values = list(filter(None, data[2:]))
msg = PprzMessage(msg_class, msg_name)
msg.set_values(values)
self.callback(ac_id, msg)
def envoi_cmd(self, phi, theta, psi, throttle):
msg = PprzMessage("datalink", "JOYSTICK_RAW")
msg['ac_id'] = self.drone.id
msg['roll'] = phi
msg['pitch'] = theta
msg['yaw'] = psi
msg['throttle'] = throttle
self._interface.send_raw_datalink(msg)
def line(shape_id):
msg_shape = PprzMessage("ground", "SHAPE")
msg_shape['id'] = shape_id
msg_shape['linecolor'] = 'purple'
msg_shape['shape'] = Shape.LINE.value
msg_shape['status'] = Status.CREATE.value
msg_shape['latarr'] = [int(43.46296 * 1e7), int(43.46216 * 1e7)]
msg_shape['lonarr'] = [int(1.272 * 1e7), int(1.2744 * 1e7)]
msg_shape['text'] = "runway"
return msg_shape
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 worldenv_cb(ac_id, msg):
"""
Callback for paparazzi WORLD_ENV requests
"""
# request location (in meters)
east, north, up = float(msg.get_field(3)),\
float(msg.get_field(4)),\
float(msg.get_field(5))
up *= -1
# convert in km + translation with mesoNH origin
weast, wnorth, wup = get_wind(east, north, up)
print("wind_est:")
print(weast)
print(wnorth)
print(wup)
msg_back=PprzMessage("ground", "WORLD_ENV")
msg_back.set_value_by_name("wind_east",weast)
msg_back.set_value_by_name("wind_north",wnorth)
msg_back.set_value_by_name("wind_up",wup)
msg_back.set_value_by_name("ir_contrast",400)
msg_back.set_value_by_name("time_scale",1)
msg_back.set_value_by_name("gps_availability",1)
ivy.send(msg_back,None)