class Ivy2RedisServer():
def __init__(self, redishost, redisport, verbose=False):
self.verbose = verbose
self.interface = IvyMessagesInterface("Ivy2Redis")
self.interface.subscribe(self.message_recv)
self.r = redis.StrictRedis(host=redishost, port=redisport, db=0)
self.keep_running = True
print("Connected to redis server %s on port %i" % (redishost, redisport))
def message_recv(self, ac_id, msg):
# if ac_id is not 0 (i.e. telemetry from an aircraft) include it in the key
# don't add it to the key for ground messages
if ac_id:
key = "{0}.{1}.{2}".format(msg.msg_class, msg.name, ac_id)
else:
key = "{0}.{1}".format(msg.msg_class, msg.name)
if self.verbose:
print("received message, key=%s, msg=%s" % (key, msg.to_json(payload_only=True)))
sys.stdout.flush()
self.r.publish(key, msg.to_json(payload_only=True))
self.r.set(key, msg.to_json(payload_only=True))
def run(self):
while self.keep_running:
time.sleep(0.1)
def stop(self):
self.keep_running = False
self.interface.shutdown()
def main():
# parse arguments
import argparse as ap
argp = ap.ArgumentParser(description="Environment variables provider "
"for Paparazzi simulation from MesoNH data")
argp.add_argument("-t",
"--time-step",
required=True,
type=int,
help="Duration of a time step between MesoNH Files.")
argp.add_argument("-f",
"--files",
required=True,
nargs='+',
help="MesoNH netCDF files, in temporal ordering")
argp.add_argument("-x",
"--origin-x",
required=False,
type=float,
default=0.,
help="Origin translation x.")
argp.add_argument("-y",
"--origin-y",
required=False,
type=float,
default=0.,
help="Origin translation y.")
argp.add_argument("-z",
"--origin-z",
required=False,
type=float,
default=0.,
help="Origin translation z.")
arguments = argp.parse_args()
print(arguments)
# register signal handler for ctrl+c to stop the program
signal.signal(signal.SIGINT, signal_handler)
# origin for translation from paparazzi to mesoNH frame
global origin
origin = np.array(
[0, arguments.origin_z, arguments.origin_x, arguments.origin_y])
# build atmosphere simulation source
global atm
atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0)
# init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS
global ivy
ivy = IvyMessagesInterface("MesoNH")
ivy.subscribe(worldenv_cb, '(.* WORLD_ENV_REQ .*)')
# wait for ivy to stop
from ivy.std_api import IvyMainLoop
signal.pause()
class IvyRequester(object):
def __init__(self, interface=None):
self._interface = interface
if interface is None:
self._interface = IvyMessagesInterface("ivy requester")
self.ac_list = []
def __del__(self):
self.shutdown()
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def get_aircrafts(self):
def aircrafts_cb(ac_id, msg):
self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
print("aircrafts: {}".format(self.ac_list))
self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
sender = 'get_aircrafts'
request_id = '42_1' # fake request id, should be PID_index
self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# hack: sleep briefly to wait for answer
sleep(0.1)
return self.ac_list
class Ivy2RedisServer():
def __init__(self, redishost, redisport, verbose=False):
self.verbose = verbose
self.interface = IvyMessagesInterface("Ivy2Redis")
self.interface.subscribe(self.message_recv)
self.r = redis.StrictRedis(host=redishost, port=redisport, db=0)
self.keep_running = True
print("Connected to redis server %s on port %i" %
(redishost, redisport))
def message_recv(self, ac_id, msg):
# if ac_id is not 0 (i.e. telemetry from an aircraft) include it in the key
# don't add it to the key for ground messages
if ac_id:
key = "{0}.{1}.{2}".format(msg.msg_class, msg.name, ac_id)
else:
key = "{0}.{1}".format(msg.msg_class, msg.name)
if self.verbose:
print("received message, key=%s, msg=%s" %
(key, msg.to_json(payload_only=True)))
sys.stdout.flush()
self.r.publish(key, msg.to_json(payload_only=True))
self.r.set(key, msg.to_json(payload_only=True))
def run(self):
while self.keep_running:
time.sleep(0.1)
def stop(self):
self.keep_running = False
self.interface.shutdown()
class IvyRequester(object):
def __init__(self, interface=None):
self._interface = interface
if interface is None:
self._interface = IvyMessagesInterface("ivy requester")
self.ac_list = []
def __del__(self):
self.shutdown()
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def get_aircrafts(self):
def aircrafts_cb(ac_id, msg):
self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
print("aircrafts: {}".format(self.ac_list))
self._interface.subscribe(aircrafts_cb, "(.*AIRCRAFTS .*)")
sender = 'get_aircrafts'
request_id = '42_1' # fake request id, should be PID_index
self._interface.send("{} {} AIRCRAFTS_REQ".format(sender, request_id))
# hack: sleep briefly to wait for answer
sleep(0.1)
return self.ac_list
class Cx10ds2ivy:
def __init__(self, verbose=False):
self.verbose = verbose
self.step = 0.1 # period in seconds
self.button_trim = False
self._cx10 = CX10DS(verbose)
# Start IVY interface
self._interface = IvyMessagesInterface("Cx10ds2ivy")
# bind to JOYSTICK message
def joystick_cb(ac_id, msg):
aileron = int(msg['axis1'])
elevator = int(msg['axis2'])
rudder = int(msg['axis3'])
if int(msg['button1']) == 255:
throttle = int(msg['axis4']) # direct throttle reading
else:
throttle = 128
direction = int(msg['button1'])-1
throttle_incr = self._cx10.valid_range(int(msg['axis4']), 0, 127)
throttle = 128 + direction * throttle_incr # up
mode = int(msg['button2'])
if msg['button4'] == '1' and not self.button_trim:
self._cx10.set_trim()
self.button_trim = (msg['button4'] == '1')
self._cx10.set_cmd(aileron,elevator,rudder,throttle,mode)
if self.verbose:
print("Throttle {0}".format(throttle))
print("Rudder {0}".format(rudder))
print("elevator {0}".format(elevator))
print("aileron {0}".format(aileron))
print("Mode {0}".format(mode))
self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
# main loop
def run(self):
try:
while True:
# TODO: make better frequency managing
self._cx10.send()
sleep(self.step)
except KeyboardInterrupt:
if self.verbose:
print("Exiting..")
self.stop()
class RadioWatchFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ROTORCRAFT_STATUS":
self.rc_status = int(msg['rc_status'])
if self.rc_status != 0 and not self.alertChannel.get_busy():
self.warn_timer = wx.CallLater(5, self.rclink_alert)
# else:
# self.notification.close()
def gui_update(self):
self.rc_statusText.SetLabel(["OK", "LOST", "REALLY LOST"][self.rc_status])
self.update_timer.Restart(UPDATE_INTERVAL)
def rclink_alert(self):
self.alertChannel.queue(self.alertSound)
self.notification.show()
time.sleep(5)
def setFont(self, control):
font = control.GetFont()
size = font.GetPointSize()
font.SetPointSize(size * 1.4)
control.SetFont(font)
def __init__(self):
wx.Frame.__init__(self, id=-1, parent=None, name=u'RCWatchFrame',
size=wx.Size(WIDTH, HEIGHT), title=u'RC Status')
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.rc_statusText = wx.StaticText(self, -1, "UNKWN")
pygame.mixer.init()
self.alertSound = pygame.mixer.Sound("crossing.wav")
self.alertChannel = pygame.mixer.Channel(False)
self.setFont(self.rc_statusText)
self.notification = pynotify.Notification("RC Link Warning!",
"RC Link status not OK!",
"dialog-warning")
self.rc_status = -1
pynotify.init("RC Status")
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(self.rc_statusText, 1, wx.EXPAND)
self.SetSizer(sizer)
sizer.Layout()
self.interface = IvyMessagesInterface("radiowatchframe")
self.interface.subscribe(self.message_recv)
self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
def main():
# parse arguments
import argparse as ap
argp = ap.ArgumentParser(description="Environment variables provider "
"for Paparazzi simulation from MesoNH data")
argp.add_argument("-t", "--time-step", required=True, type=int,
help="Duration of a time step between MesoNH Files.")
argp.add_argument("-f", "--files", required=True, nargs='+',
help="MesoNH netCDF files, in temporal ordering")
argp.add_argument("-x", "--origin-x", required=False, type=float,
default=0.,
help="Origin translation x.")
argp.add_argument("-y", "--origin-y", required=False, type=float,
default=0.,
help="Origin translation y.")
argp.add_argument("-z", "--origin-z", required=False, type=float,
default=0.,
help="Origin translation z.")
arguments = argp.parse_args()
print(arguments)
# register signal handler for ctrl+c to stop the program
signal.signal(signal.SIGINT, signal_handler)
# origin for translation from paparazzi to mesoNH frame
global origin
origin = np.array([0, arguments.origin_z, arguments.origin_x, arguments.origin_y])
# build atmosphere simulation source
global atm
atm = MesoNHAtmosphere(arguments.files, arguments.time_step, tinit=0)
# init ivy and register callback for WORLD_ENV_REQ and NPS_SPEED_POS
global ivy
ivy = IvyMessagesInterface("MesoNH");
ivy.subscribe(worldenv_cb,'(.* WORLD_ENV_REQ .*)')
# wait for ivy to stop
from ivy.std_api import IvyMainLoop
signal.pause()
class RosIvyMessagesInterface(RosMessagesInterface):
def __init__(self,
agent_name=None,
start_ivy=True,
verbose=False,
ivy_bus=IVY_BUS):
self.interface = IvyMessagesInterface(agent_name, start_ivy, verbose,
ivy_bus)
RosMessagesInterface.__init__(self)
# only subscribe to telemetry messages eg. starting with AC_ID
self.interface.subscribe(callback=self.to_ros,
regex_or_msg='(^[0-9]+ .*)')
def from_ros(self, ros_msg):
pprz_msg = self.converter.ros2pprz(ros_msg)
self.interface.send(pprz_msg)
def to_ros(self, sender_id, pprz_msg):
ros_msg = self.converter.pprz2ros(sender_id, pprz_msg)
self.pub.publish(ros_msg)
class uEyeIvy(uEyePprzlink):
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 __exit__(self):
if self.pprzivy is not None:
self.stop()
def stop(self):
self.pprzivy.shutdown()
self.pprzivy = None
uEyePprzlink.stop(self)
def run(self):
try:
while True:
if self.new_msg and self.cam is not None:
ret = self.cam.freeze_video(True)
if ret == ueye.IS_SUCCESS:
self.verbose_print("Freeze done")
img = ImageData(self.cam.handle(), self.buff)
self.process_image(img, 0)
self.verbose_print("Process done")
else:
self.verbose_print('Freeze fail with {%d}' % ret)
self.new_msg = False
else:
time.sleep(0.1)
except (KeyboardInterrupt, SystemExit):
pass
class GVFFrame(wx.Frame):
def __init__(self, ac_id):
wx.Frame.__init__(self, id=-1, parent=None, \
name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \
style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field')
# Vehicle variables
self.ac_id = ac_id
self.course = 0
self.yaw = 0
self.XY = np.array([0, 0])
# Desired trajectory
self.timer_traj = 0 # We do not update the traj every time we receive a msg
self.timer_traj_lim = 7 # (7+1) * 0.25secs
self.s = 0
self.ke = 0
self.map_gvf = map2d(np.array([0, 0]), 150000)
self.traj = None
# Frame
self.canvas = FigureCanvas(self, -1, self.map_gvf.fig)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.redraw_timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer)
self.redraw_timer.Start(100)
# Ivy
self.interface = IvyMessagesInterface("GVF")
self.interface.subscribe(self.message_recv)
settings = PaparazziACSettings(ac_id)
def message_recv(self, ac_id, msg):
if int(ac_id) == self.ac_id:
if msg.name == 'GPS':
self.course = int(msg.get_field(3))*np.pi/1800
if msg.name == 'NAVIGATION':
self.XY[0] = float(msg.get_field(2))
self.XY[1] = float(msg.get_field(3))
if msg.name == 'ATTITUDE':
self.yaw = float(msg.get_field(1))
if msg.name == 'GVF':
self.gvf_error = float(msg.get_field(0))
# Straight line
if int(msg.get_field(1)) == 0 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).split(',')]
a = param[0]
b = param[1]
c = param[2]
self.traj = traj_line(np.array([-100,100]), a, b, c)
self.traj.vector_field(self.traj.XYoff, self.map_gvf.area, \
self.s, self.ke)
# Ellipse
if int(msg.get_field(1)) == 1 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).split(',')]
ex = param[0]
ey = param[1]
ea = param[2]
eb = param[3]
ealpha = param[4]
self.traj = traj_ellipse(np.array([ex, ey]), ealpha, ea, eb)
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.ke)
# Sin
if int(msg.get_field(1)) == 2 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4).split(',')]
a = param[0]
b = param[1]
alpha = param[2]
w = param[3]
off = param[4]
A = param[5]
self.traj = traj_sin(np.array([-100, 100]), a, b, alpha, \
w, off, A)
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.ke)
self.timer_traj = self.timer_traj + 1
if self.timer_traj > self.timer_traj_lim:
self.timer_traj = 0
def draw_gvf(self, XY, yaw, course):
if self.traj is not None:
self.map_gvf.draw(XY, yaw, course, self.traj)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
def OnRedrawTimer(self, event):
self.draw_gvf(self.XY, self.yaw, self.course)
self.canvas.draw()
class MessagesFrame(wx.Frame):
def message_recv(self, ac_id, msg):
"""Handle incoming messages
Callback function for IvyMessagesInterface
:param ac_id: aircraft id
:type ac_id: int
:param msg: message
:type msg: PprzMessage
"""
# only show messages of the requested class
if msg.msg_class != self.msg_class:
return
if ac_id in self.aircrafts and msg.name in self.aircrafts[ac_id].messages:
if time.time() - self.aircrafts[ac_id].messages[msg.name].last_seen < 0.2:
return
wx.CallAfter(self.gui_update, ac_id, msg)
def find_page(self, book, name):
if book.GetPageCount() < 1:
return 0
start = 0
end = book.GetPageCount()
while start < end:
if book.GetPageText(start) >= name:
return start
start += 1
return start
def update_leds(self):
wx.CallAfter(self.update_leds_real)
def update_leds_real(self):
for ac_id in self.aircrafts:
aircraft = self.aircrafts[ac_id]
for msg_str in aircraft.messages:
message = aircraft.messages[msg_str]
if message.last_seen + 0.2 < time.time():
aircraft.messages_book.SetPageImage(message.index, 0)
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
def setup_image_list(self, notebook):
imageList = wx.ImageList(24, 24)
image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
notebook.AssignImageList(imageList)
def add_new_aircraft(self, ac_id):
self.aircrafts[ac_id] = Aircraft(ac_id)
ac_panel = wx.Panel(self.notebook, -1)
self.notebook.AddPage(ac_panel, str(ac_id))
messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT)
self.setup_image_list(messages_book)
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(messages_book, 1, wx.EXPAND)
ac_panel.SetSizer(sizer)
sizer.Layout()
self.aircrafts[ac_id].messages_book = messages_book
def add_new_message(self, aircraft, msg_class, name):
messages_book = aircraft.messages_book
aircraft.messages[name] = Message(msg_class, name)
field_panel = wx.Panel(messages_book)
grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames), 2)
index = self.find_page(messages_book, name)
messages_book.InsertPage(index, field_panel, name, imageId=1)
aircraft.messages[name].index = index
# update indexes of pages which are to be moved
for message_name in aircraft.messages:
aircraft.messages[message_name].index = self.find_page(messages_book, message_name)
for field_name in aircraft.messages[name].fieldnames:
name_text = wx.StaticText(field_panel, -1, field_name)
size = name_text.GetSize()
size.x = LABEL_WIDTH
name_text.SetMinSize(size)
grid_sizer.Add(name_text, 1, wx.ALL, BORDER)
value_control = wx.StaticText(field_panel, -1, "42", style=wx.EXPAND)
size = value_control.GetSize()
size.x = LABEL_WIDTH
value_control.SetMinSize(size)
grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER)
if wx.MAJOR_VERSION > 2:
if grid_sizer.IsColGrowable(1):
grid_sizer.AddGrowableCol(1)
else:
grid_sizer.AddGrowableCol(1)
aircraft.messages[name].field_controls.append(value_control)
field_panel.SetAutoLayout(True)
field_panel.SetSizer(grid_sizer)
field_panel.Layout()
def gui_update(self, ac_id, msg):
if ac_id not in self.aircrafts:
self.add_new_aircraft(ac_id)
aircraft = self.aircrafts[ac_id]
if msg.name not in aircraft.messages:
self.add_new_message(aircraft, msg.msg_class, msg.name)
aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index, 1)
self.aircrafts[ac_id].messages[msg.name].last_seen = time.time()
for index in range(0, len(msg.fieldvalues)):
aircraft.messages[msg.name].field_controls[index].SetLabel(str(msg.get_field(index)))
def __init__(self, msg_class="telemetry"):
wx.Frame.__init__(self, id=-1, parent=None, name=u'MessagesFrame', size=wx.Size(WIDTH, HEIGHT), style=wx.DEFAULT_FRAME_STYLE, title=u'Messages')
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.notebook = wx.Notebook(self)
self.aircrafts = {}
sizer = wx.BoxSizer(wx.HORIZONTAL)
sizer.Add(self.notebook, 1, wx.EXPAND)
self.SetSizer(sizer)
sizer.Layout()
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
self.msg_class = msg_class
self.interface = IvyMessagesInterface("Paparazzi Messages Viewer")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.timer.cancel()
self.interface.shutdown()
self.Destroy()
class FormationControl:
def __init__(self,
config,
freq=10.,
use_ground_ref=False,
ignore_geo_fence=False,
verbose=False):
self.config = config
self.step = 1. / freq
self.sens = self.config['sensitivity']
self.use_ground_ref = use_ground_ref
self.enabled = True # run control by default
self.ignore_geo_fence = ignore_geo_fence
self.verbose = verbose
self.ids = self.config['ids']
self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids]
self.joystick = FC_Joystick()
self.altitude = 2.0 # starts from 1 m high
self.scale = 1.0
self.B = np.array(self.config['topology'])
self.d = np.array(self.config['desired_distances'])
self.t1 = np.array(self.config['motion']['t1'])
self.t2 = np.array(self.config['motion']['t2'])
self.r1 = np.array(self.config['motion']['r1'])
self.r2 = np.array(self.config['motion']['r2'])
self.k = np.array(self.config['gains'])
if self.B.size == 2:
self.B.shape = (2, 1)
# Check formation settings
if len(self.ids) != np.size(self.B, 0):
print(
"The number of rotorcrafts in the topology and ids do not match"
)
return
if np.size(self.d) != np.size(self.B, 1):
print(
"The number of links in the topology and desired distances do not match"
)
return
#if np.size(self.d) != np.size(self.m,1):
# print("The number of (columns) motion parameters and relative vectors do not match")
# return
#if np.size(self.m, 0) != 8:
# print("The number of (rows) motion parameters must be eight")
# return
if self.config['3D'] == True:
print("3D formation is not supported yet")
return
# Start IVY interface
self._interface = IvyMessagesInterface("Formation Control Rotorcrafts")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
if not self.use_ground_ref:
self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = -float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = -float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
if self.use_ground_ref:
self._interface.subscribe(ground_ref_cb,
PprzMessage("ground", "GROUND_REF"))
# bind to JOYSTICK message
def joystick_cb(ac_id, msg):
self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (
self.sens['alt_max'] - self.sens['alt_min']) / 127.
if msg['button1'] == '1' and not self.joystick.button1:
self.scale = min(self.scale + self.sens['scale'],
self.sens['scale_max'])
if msg['button2'] == '1' and not self.joystick.button2:
self.scale = max(self.scale - self.sens['scale'],
self.sens['scale_min'])
if msg['button4'] == '1' and not self.joystick.button4:
self.enabled = False
if msg['button3'] == '1' and not self.joystick.button3:
self.enabled = True
self.joystick.button1 = (msg['button1'] == '1')
self.joystick.button2 = (msg['button2'] == '1')
self.joystick.button3 = (msg['button3'] == '1')
self.joystick.button4 = (msg['button4'] == '1')
self._interface.subscribe(joystick_cb,
PprzMessage("ground", "JOYSTICK"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in list(self.config.keys()):
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min']
or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min']
or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min']
or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id,
" is out of the fence (", rc.X, ", ",
geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i + 1] = rc.X[1]
V[i] = rc.V[0]
V[i + 1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0, :]
jtilde_mu_t1 = self.joystick.trans * self.t1[1, :]
jmu_r1 = self.joystick.rot * self.r1[0, :]
jtilde_mu_r1 = self.joystick.rot * self.r1[1, :]
jmu_t2 = self.joystick.trans2 * self.t2[0, :]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1, :]
jmu_r2 = self.joystick.rot2 * self.r2[0, :]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1, :]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1], Zh[0]])
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * (Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(
jmu_r1[0]) * la.kron(
Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1),
np.eye(2)).dot(Zhr)
else:
U = -self.k[1] * V - self.k[0] * Bb.dot(Dz).dot(Dzt).dot(
E) + self.k[1] * Avb.dot(Zh) + la.kron(
Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print("Error distances: " +
str(E).replace('[', '').replace(']', ''))
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i + 1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i + 2
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
for rc in self.rotorcrafts:
rc.timeout = rc.timeout + self.step
# Run the formation algorithm
if self.enabled:
self.formation()
except KeyboardInterrupt:
self.stop()
class EnergyMonFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ENERGY":
self.bat = EnergyMessage(msg)
self.cell.fill_from_energy_msg(self.bat)
wx.CallAfter(self.update)
elif msg.name == "TEMP_ADC":
self.temp = TempMessage(msg)
self.cell.fill_from_temp_msg(self.temp)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def StatusBox(self, dc, nr, txt, percent, color):
if percent < 0:
percent = 0
if percent > 1:
percent = 1
boxw = self.stat
tdx = int(boxw * 10.0 / 300.0)
tdy = int(boxw * 6.0 / 300.0)
boxh = int(boxw * 40.0 / 300.0)
boxw = self.stat - 2 * tdx
spacing = boxh + 10
dc.SetPen(wx.Pen(wx.Colour(0, 0, 0)))
dc.SetBrush(wx.Brush(wx.Colour(220, 220, 220)))
dc.DrawRectangle(tdx, int(nr * spacing + tdx), int(boxw), boxh)
if color < 0.2:
dc.SetBrush(wx.Brush(wx.Colour(250, 0, 0)))
elif color < 0.6:
dc.SetBrush(wx.Brush(wx.Colour(250, 180, 0)))
else:
dc.SetBrush(wx.Brush(wx.Colour(0, 250, 0)))
# dc.DrawLine(200,50,350,50)
dc.DrawRectangle(tdx, int(nr * spacing + tdx), int(boxw * percent),
boxh)
dc.DrawText(txt, 18, int(nr * spacing + tdy + tdx))
def plot_x(self, x):
return int(self.stat + self.tdx + x *
(self.w - self.stat - 2 * self.tdx))
def plot_y(self, y):
return int(self.tdx + (1 - y) * (self.h - self.tdx - self.tdx))
def plot(self, dc, i1, i2):
dc.DrawLine(self.plot_x(i1[1] / 3500),
self.plot_y((i1[0] - 2.5) / (4.2 - 2.5)),
self.plot_x(i2[1] / 3500),
self.plot_y((i2[0] - 2.5) / (4.2 - 2.5)))
def DischargePlot(self, dc):
self.tdx = int(self.stat * 10.0 / 300.0)
dc.SetPen(wx.Pen(wx.Colour(0, 0, 0), 1))
dc.SetBrush(wx.Brush(wx.Colour(250, 250, 250)))
dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0),
self.w - self.stat - 2 * self.tdx,
self.h - 2 * self.tdx)
for i in range(0, 5):
dc.DrawLine(self.plot_x(0.0), self.plot_y(i / 5.0),
self.plot_x(1.0), self.plot_y(i / 5.0))
for i in range(0, 7):
dc.DrawLine(self.plot_x(i / 7.0), self.plot_y(0),
self.plot_x(i / 7.0), self.plot_y(1))
dc.SetPen(wx.Pen(wx.Colour(255, 180, 0), 4))
dc.DrawLine(self.plot_x(self.cell.model / 3500), self.plot_y(0),
self.plot_x(self.cell.model / 3500), self.plot_y(1))
dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()),
self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc()))
thickness = 3
dc.SetPen(wx.Pen(wx.Colour(0, 0, 0), thickness))
li = bat.batmodel[0, [0, 1]]
for i in bat.batmodel[:, [0, 1]]:
self.plot(dc, li, i)
li = i
dc.SetPen(wx.Pen(wx.Colour(0, 0, 255), thickness))
li = bat.batmodel[0, [0, 2]]
for i in bat.batmodel[:, [0, 2]]:
self.plot(dc, li, i)
li = i
dc.SetPen(wx.Pen(wx.Colour(0, 255, 0), thickness))
li = bat.batmodel[0, [0, 3]]
for i in bat.batmodel[:, [0, 3]]:
self.plot(dc, li, i)
li = i
dc.SetPen(wx.Pen(wx.Colour(255, 255, 0), thickness))
li = bat.batmodel[0, [0, 4]]
for i in bat.batmodel[:, [0, 4]]:
self.plot(dc, li, i)
li = i
dc.SetPen(wx.Pen(wx.Colour(255, 0, 0), thickness))
li = bat.batmodel[0, [0, 5]]
for i in bat.batmodel[:, [0, 5]]:
self.plot(dc, li, i)
li = i
def OnPaint(self, e):
# Automatic Scaling
w = self.w
h = self.h
self.stat = int(w / 4)
if self.stat < 100:
self.stat = 100
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
fontscale = int(w * 11.0 / 800.0)
if fontscale < 6:
fontscale = 6
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL,
wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
self.StatusBox(dc, 0, self.cell.get_volt(), self.cell.get_volt_perc(),
self.cell.get_volt_color())
self.StatusBox(dc, 1, self.cell.get_current(),
self.cell.get_current_perc(),
self.cell.get_current_color())
self.StatusBox(dc, 2, self.cell.get_energy(),
self.cell.get_energy_perc(),
self.cell.get_energy_color())
self.StatusBox(dc, 3, self.cell.get_mah_from_volt(),
self.cell.get_energy_perc(),
self.cell.get_energy_color())
self.StatusBox(dc, 4, self.cell.get_temp(), self.cell.get_temp_perc(),
self.cell.get_temp_color())
self.DischargePlot(dc)
def __init__(self):
self.w = WIDTH
self.h = WIDTH + BARH
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'EnergyMonFrame',
size=wx.Size(self.w, self.h),
title=u'Energy Monitoring')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(
PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico",
wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.bat = {}
self.temp = {}
self.cell = BatteryCell()
self.interface = IvyMessagesInterface("energymonframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class MessagePicker(wx.Frame):
def __init__(self, parent, callback, ivy_interface=None):
wx.Frame.__init__(self, parent, name="MessagePicker", title=u'Message Picker', size=wx.Size(320,640))
self.aircrafts = {}
self.callback = callback
self.tree = wx.TreeCtrl(self)
self.root = self.tree.AddRoot("Telemetry")
self.tree.Bind(wx.EVT_LEFT_DCLICK, self.OnDoubleClick)
self.tree.Bind(wx.EVT_CHAR, self.OnKeyChar)
self.Bind(wx.EVT_CLOSE, self.OnClose)
if ivy_interface is None:
self.message_interface = IvyMessagesInterface("MessagePicker")
else:
self.message_interface = ivy_interface
self.message_interface.subscribe(self.msg_recv)
def OnClose(self, event):
# if we have a parent (like the plotpanel) only hide instead of shutdown
if self.GetParent() is not None:
self.Hide()
else:
self.message_interface.shutdown()
self.Destroy()
def msg_recv(self, ac_id, msg):
if msg.msg_class != "telemetry":
return
self.tree.Expand(self.root)
if ac_id not in self.aircrafts:
ac_node = self.tree.AppendItem(self.root, str(ac_id))
self.aircrafts[ac_id] = Aircraft(ac_id)
self.aircrafts[ac_id].messages_book = ac_node
aircraft = self.aircrafts[ac_id]
ac_node = aircraft.messages_book
if msg.name not in aircraft.messages:
msg_node = self.tree.AppendItem(ac_node, str(msg.name))
self.tree.SortChildren(ac_node)
aircraft.messages[msg.name] = Message("telemetry", msg.name)
for field in aircraft.messages[msg.name].fieldnames:
item = self.tree.AppendItem(msg_node, field)
def OnKeyChar(self, event):
if event.GetKeyCode() != 13:
return False
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
def OnDoubleClick(self, event):
node = self.tree.GetSelection()
field_name = self.tree.GetItemText(node)
parent = self.tree.GetItemParent(node)
message_name = self.tree.GetItemText(parent)
grandparent = self.tree.GetItemParent(parent)
ac_id = self.tree.GetItemText(grandparent)
if node == self.root or parent == self.root or grandparent == self.root:
# if not leaf, double click = expand
if self.tree.IsExpanded(node):
self.tree.Collapse(node)
else:
self.tree.Expand(node)
return
self.callback(int(ac_id), message_name, field_name)
class MagCalibrator(object):
def __init__(self, plot_results=True, verbose=False):
self._interface = IvyMessagesInterface("calib_mag")
self.plotter = MagPlot()
self.data = []
self.flt_meas = []
self.p0 = np.array([0, 0, 0, 0, 0, 0])
self.optimization_done = False
self.plot_results = plot_results
def start_collect(self):
self._interface.subscribe(self.message_recv, "(.*IMU_MAG_RAW.*)")
def stop_collect(self):
self._interface.unsubscribe_all()
def message_recv(self, ac_id, msg):
self.data.append(np.array([int(v) for v in msg.fieldvalues]))
if self.plot_results:
self.plotter.add_data((map(int, msg.fieldvalues)))
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def calc_min_max_guess(self):
if len(self.data) > 3:
# filter out noisy measurements?
self.flt_meas = np.array(self.data)
self.p0 = calibration_utils.get_min_max_guess(self.flt_meas, 1.0)
def print_min_max_guess(self):
self.calc_min_max_guess()
if self.data:
print(
"Current guess from %d measurements: neutral [%d, %d, %d], scale [%.3f, %.3f, %.3f]"
% (len(self.flt_meas), int(round(self.p0[0])),
int(round(self.p0[1])), int(round(self.p0[2])),
self.p0[3] * 2**11, self.p0[4] * 2**11, self.p0[5] * 2**11))
def calibrate(self):
self.calc_min_max_guess()
if len(self.flt_meas) < 10:
logging.warning("Not enough measurements")
return
cp0, np0 = calibration_utils.scale_measurements(self.flt_meas, self.p0)
logging.info("initial guess : avg " + str(np0.mean()) + " std " +
str(np0.std()))
calibration_utils.print_xml(self.p0, "MAG", 11)
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, cov, info, msg, success = optimize.leastsq(err_func,
self.p0[:],
args=(self.flt_meas,
1.0),
full_output=1)
self.optimization_done = success in [1, 2, 3, 4]
if not self.optimization_done:
logging.warning("Optimization error: ", msg)
cp1, np1 = calibration_utils.scale_measurements(self.flt_meas, p1)
if self.optimization_done:
logging.info("optimized guess : avg " + str(np1.mean()) + " std " +
str(np1.std()))
calibration_utils.print_xml(p1, "MAG", 11)
else:
logging.info("last iteration of failed optimized guess : avg " +
str(np1.mean()) + " std " + str(np1.std()))
if self.plot_results:
calibration_utils.plot_results("MAG",
np.array(self.data),
range(len(self.data)),
self.flt_meas,
cp0,
np0,
cp1,
np1,
1.0,
blocking=False)
calibration_utils.plot_mag_3d(self.flt_meas, cp1, p1)
class DistanceCounterFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "INS":
self.msg_count = self.msg_count + 1
newx = float(msg.get_field(0)) / 256.0
newy = float(msg.get_field(1)) / 256.0
moved = ((newx - self.ins_msg_x) ** 2 + (newy - self.ins_msg_y) ** 2)
if self.init == 0:
self.init = 1
elif self.running:
self.distance = self.distance + math.sqrt(moved)
self.ins_msg_x = newx
self.ins_msg_y = newy
self.ins_msg_z = msg.get_field(2)
# graphical update
wx.CallAfter(self.update)
if msg.name == "ROTORCRAFT_STATUS":
self.msg_count_time = self.msg_count_time + 1
time_new = float(msg['cpu_time'])
if time_new > self.time_old and self.time_old != 0 and self.running:
self.time_elapsed += time_new - self.time_old
self.time_old = time_new
# graphical update
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
# Paint Area
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
font = wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("INS Packets:" + str(self.msg_count),2,2)
dc.DrawText("Data: " + str(self.ins_msg_x) + ", " + str(self.ins_msg_y) + ", " + str(self.ins_msg_z) + ".",2,22)
dc.DrawText("Distance: " + str(round(float(self.distance)/1.0,2)) + " m",2,22+20)
dc.DrawText("Time elapsed: " + str(self.time_elapsed) + "s",2,22+20+20)
if self.running:
dc.DrawText("Counter running", 150, 22+20)
else:
dc.DrawText("Counter paused", 150, 22+20)
def onStartStop(self, event):
self.running = not self.running
self.Refresh()
def onReset(self, event):
self.time_old = 0
self.time_elapsed = 0
self.distance = 0
self.init = 0
self.Refresh()
return
def __init__(self, _settings):
# Command line arguments
self.settings = _settings
# Statistics
self.data = { 'packets': 0, 'bytes': 0}
self.w = WIDTH
self.h = WIDTH
# Frame
wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter',
size=wx.Size(self.w, self.h), title=u'Distance Counter')
start_stop_button = wx.Button(self, wx.ID_ANY, 'Start/Pause', (150, 58),size=(90, 25))
start_stop_button.Bind(wx.EVT_BUTTON, self.onStartStop)
reset_button = wx.Button(self, wx.ID_ANY, 'Reset', (245, 58), size=(50, 25))
reset_button.Bind(wx.EVT_BUTTON, self.onReset)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
# IVY
self.interface = IvyMessagesInterface("DistanceCounter")
self.interface.subscribe(self.message_recv)
self.msg_count = 0
self.msg_count_time = 0
self.distance = 0
self.time_old = 0
self.time_elapsed = 0
self.ins_msg_x = 0
self.ins_msg_y = 0
self.ins_msg_z = 0
self.init = 0
self.running = True
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class AtcFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "INS_REF":
self.qfe = round(float(msg['baro_qfe']) / 100.0, 1)
wx.CallAfter(self.update)
elif msg.name == "ROTORCRAFT_FP_MIN":
self.gspeed = round(float(msg['gspeed']) / 100.0 * 3.6 / 1.852, 1)
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084, 1)
wx.CallAfter(self.update)
elif msg.name == "ROTORCRAFT_FP":
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084, 1)
wx.CallAfter(self.update)
elif msg.name == "AIR_DATA":
self.airspeed = round(float(msg['airspeed']) * 3.6 / 1.852, 1)
self.qnh = round(float(msg['qnh']), 1)
self.amsl = round(float(msg['amsl_baro']) * 3.28084, 1)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnPaint(self, e):
tdx = 10
tdy = 80
w = self.w
h = self.w
dc = wx.PaintDC(self)
#brush = wx.Brush("white")
#dc.SetBackground(brush)
#dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
#dc.DrawCircle(w/2,w/2,w/2-1)
font = wx.Font(40, wx.ROMAN, wx.BOLD, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("Airspeed: " + str(self.airspeed) + " kt", tdx, tdx)
dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt", tdx,
tdx + tdy * 1)
dc.DrawText("AMSL: " + str(self.amsl) + " ft", tdx, tdx + tdy * 2)
dc.DrawText("QNH: " + str(self.qnh) + " ", tdx, tdx + tdy * 3)
dc.DrawText("ALT: " + str(self.alt) + " ", tdx, tdx + tdy * 4)
dc.DrawText("QFE: " + str(self.qfe) + " ", tdx, tdx + tdy * 5)
#dc.DrawText("HMSL: " + str(self.hmsl) + " ft",tdx,tdx+tdy*6)
#c = wx.Colour(0,0,0)
#dc.SetBrush(wx.Brush(c, wx.SOLID))
#dc.DrawCircle(int(w/2),int(w/2),10)
def __init__(self):
self.w = 900
self.h = 700
self.airspeed = 0
self.amsl = 0
self.qnh = 0
self.qfe = 0
self.alt = 0
#self.hmsl = 0;
self.gspeed = 0
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'ATC Center',
size=wx.Size(self.w, self.h),
title=u'ATC Center')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico",
wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("ATC-Center")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class RadioWatchFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ROTORCRAFT_STATUS":
self.rc_status = int(msg['rc_status'])
if self.rc_status != 0 and not self.alertChannel.get_busy():
self.warn_timer = wx.CallLater(5, self.rclink_alert)
# else:
# self.notification.close()
def gui_update(self):
self.rc_statusText.SetLabel(["OK", "LOST",
"REALLY LOST"][self.rc_status])
self.update_timer.Restart(UPDATE_INTERVAL)
def rclink_alert(self):
self.alertChannel.queue(self.alertSound)
self.notification.show()
time.sleep(5)
def setFont(self, control):
font = control.GetFont()
size = font.GetPointSize()
font.SetPointSize(size * 1.4)
control.SetFont(font)
def __init__(self):
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'RCWatchFrame',
size=wx.Size(WIDTH, HEIGHT),
title=u'RC Status')
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.rc_statusText = wx.StaticText(self, -1, "UNKWN")
pygame.mixer.init()
self.alertSound = pygame.mixer.Sound("crossing.wav")
self.alertChannel = pygame.mixer.Channel(False)
self.setFont(self.rc_statusText)
self.notification = pynotify.Notification("RC Link Warning!",
"RC Link status not OK!",
"dialog-warning")
self.rc_status = -1
pynotify.init("RC Status")
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(self.rc_statusText, 1, wx.EXPAND)
self.SetSizer(sizer)
sizer.Layout()
self.interface = IvyMessagesInterface("radiowatchframe")
self.interface.subscribe(self.message_recv)
self.update_timer = wx.CallLater(UPDATE_INTERVAL, self.gui_update)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class FormationControl:
def __init__(self, config, freq=10., use_ground_ref=False, ignore_geo_fence=False, verbose=False):
self.config = config
self.step = 1. / freq
self.sens = self.config['sensitivity']
self.use_ground_ref = use_ground_ref
self.enabled = True # run control by default
self.ignore_geo_fence = ignore_geo_fence
self.verbose = verbose
self.ids = self.config['ids']
self.rotorcrafts = [FC_Rotorcraft(i) for i in self.ids]
self.joystick = FC_Joystick()
self.altitude = 2.0 # starts from 1 m high
self.scale = 1.0
self.B = np.array(self.config['topology'])
self.d = np.array(self.config['desired_distances'])
self.t1 = np.array(self.config['motion']['t1'])
self.t2 = np.array(self.config['motion']['t2'])
self.r1 = np.array(self.config['motion']['r1'])
self.r2 = np.array(self.config['motion']['r2'])
self.k = np.array(self.config['gains'])
if self.B.size == 2:
self.B.shape = (2,1)
# Check formation settings
if len(self.ids) != np.size(self.B, 0):
print("The number of rotorcrafts in the topology and ids do not match")
return
if np.size(self.d) != np.size(self.B, 1):
print("The number of links in the topology and desired distances do not match")
return
#if np.size(self.d) != np.size(self.m,1):
# print("The number of (columns) motion parameters and relative vectors do not match")
# return
#if np.size(self.m, 0) != 8:
# print("The number of (rows) motion parameters must be eight")
# return
if self.config['3D'] == True:
print("3D formation is not supported yet")
return
# Start IVY interface
self._interface = IvyMessagesInterface("Formation Control Rotorcrafts")
# bind to INS message
def ins_cb(ac_id, msg):
if ac_id in self.ids and msg.name == "INS":
rc = self.rotorcrafts[self.ids.index(ac_id)]
i2p = 1. / 2**8 # integer to position
i2v = 1. / 2**19 # integer to velocity
rc.X[0] = float(msg['ins_x']) * i2p
rc.X[1] = float(msg['ins_y']) * i2p
rc.X[2] = float(msg['ins_z']) * i2p
rc.V[0] = float(msg['ins_xd']) * i2v
rc.V[1] = float(msg['ins_yd']) * i2v
rc.V[2] = float(msg['ins_zd']) * i2v
rc.timeout = 0
rc.initialized = True
if not self.use_ground_ref:
self._interface.subscribe(ins_cb, PprzMessage("telemetry", "INS"))
# bind to GROUND_REF message
def ground_ref_cb(ground_id, msg):
ac_id = int(msg['ac_id'])
if ac_id in self.ids:
rc = self.rotorcrafts[self.ids.index(ac_id)]
# X and V in NED
rc.X[0] = float(msg['pos'][1])
rc.X[1] = float(msg['pos'][0])
rc.X[2] = -float(msg['pos'][2])
rc.V[0] = float(msg['speed'][1])
rc.V[1] = float(msg['speed'][0])
rc.V[2] = -float(msg['speed'][2])
rc.timeout = 0
rc.initialized = True
if self.use_ground_ref:
self._interface.subscribe(ground_ref_cb, PprzMessage("ground", "GROUND_REF"))
# bind to JOYSTICK message
def joystick_cb(ac_id, msg):
self.joystick.trans = float(msg['axis1']) * self.sens['t1'] / 127.
self.joystick.trans2 = float(msg['axis2']) * self.sens['t2'] / 127.
self.joystick.rot = float(msg['axis3']) * self.sens['r1'] / 127.
self.altitude = self.sens['alt_min'] + float(msg['axis4']) * (self.sens['alt_max'] - self.sens['alt_min']) / 127.
if msg['button1'] == '1' and not self.joystick.button1:
self.scale = min(self.scale + self.sens['scale'], self.sens['scale_max'])
if msg['button2'] == '1' and not self.joystick.button2:
self.scale = max(self.scale - self.sens['scale'], self.sens['scale_min'])
if msg['button4'] == '1' and not self.joystick.button4:
self.enabled = False
if msg['button3'] == '1' and not self.joystick.button3:
self.enabled = True
self.joystick.button1 = (msg['button1'] == '1')
self.joystick.button2 = (msg['button2'] == '1')
self.joystick.button3 = (msg['button3'] == '1')
self.joystick.button4 = (msg['button4'] == '1')
self._interface.subscribe(joystick_cb, PprzMessage("ground", "JOYSTICK"))
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def formation(self):
'''
formation control algorithm
'''
ready = True
for rc in self.rotorcrafts:
if not rc.initialized:
if self.verbose:
print("Waiting for state of rotorcraft ", rc.id)
sys.stdout.flush()
ready = False
if rc.timeout > 0.5:
if self.verbose:
print("The state msg of rotorcraft ", rc.id, " stopped")
sys.stdout.flush()
ready = False
if rc.initialized and 'geo_fence' in self.config.keys():
geo_fence = self.config['geo_fence']
if not self.ignore_geo_fence:
if (rc.X[0] < geo_fence['x_min'] or rc.X[0] > geo_fence['x_max']
or rc.X[1] < geo_fence['y_min'] or rc.X[1] > geo_fence['y_max']
or rc.X[2] < geo_fence['z_min'] or rc.X[2] > geo_fence['z_max']):
if self.verbose:
print("The rotorcraft", rc.id, " is out of the fence (", rc.X, ", ", geo_fence, ")")
sys.stdout.flush()
ready = False
if not ready:
return
dim = 2 * len(self.rotorcrafts)
X = np.zeros(dim)
V = np.zeros(dim)
U = np.zeros(dim)
i = 0
for rc in self.rotorcrafts:
X[i] = rc.X[0]
X[i+1] = rc.X[1]
V[i] = rc.V[0]
V[i+1] = rc.V[1]
i = i + 2
Bb = la.kron(self.B, np.eye(2))
Z = Bb.T.dot(X)
Dz = rf.make_Dz(Z, 2)
Dzt = rf.make_Dzt(Z, 2, 1)
Dztstar = rf.make_Dztstar(self.d * self.scale, 2, 1)
Zh = rf.make_Zh(Z, 2)
E = rf.make_E(Z, self.d * self.scale, 2, 1)
# Shape and motion control
jmu_t1 = self.joystick.trans * self.t1[0,:]
jtilde_mu_t1 = self.joystick.trans * self.t1[1,:]
jmu_r1 = self.joystick.rot * self.r1[0,:]
jtilde_mu_r1 = self.joystick.rot * self.r1[1,:]
jmu_t2 = self.joystick.trans2 * self.t2[0,:]
jtilde_mu_t2 = self.joystick.trans2 * self.t2[1,:]
jmu_r2 = self.joystick.rot2 * self.r2[0,:]
jtilde_mu_r2 = self.joystick.rot2 * self.r2[1,:]
Avt1 = rf.make_Av(self.B, jmu_t1, jtilde_mu_t1)
Avt1b = la.kron(Avt1, np.eye(2))
Avr1 = rf.make_Av(self.B, jmu_r1, jtilde_mu_r1)
Avr1b = la.kron(Avr1, np.eye(2))
Avt2 = rf.make_Av(self.B, jmu_t2, jtilde_mu_t2)
Avt2b = la.kron(Avt2, np.eye(2))
Avr2 = rf.make_Av(self.B, jmu_r2, jtilde_mu_r2)
Avr2b = la.kron(Avr2, np.eye(2))
Avb = Avt1b + Avr1b + Avt2b + Avr2b
Avr = Avr1 + Avr2
if self.B.size == 2:
Zhr = np.array([-Zh[1],Zh[0]])
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*(Avt1b.dot(Zh) + Avt2b.dot(Zhr)) + np.sign(jmu_r1[0])*la.kron(Avr1.dot(Dztstar).dot(self.B.T).dot(Avr1), np.eye(2)).dot(Zhr)
else:
U = -self.k[1]*V - self.k[0]*Bb.dot(Dz).dot(Dzt).dot(E) + self.k[1]*Avb.dot(Zh) + la.kron(Avr.dot(Dztstar).dot(self.B.T).dot(Avr), np.eye(2)).dot(Zh)
if self.verbose:
#print "Positions: " + str(X).replace('[','').replace(']','')
#print "Velocities: " + str(V).replace('[','').replace(']','')
#print "Acceleration command: " + str(U).replace('[','').replace(']','')
print "Error distances: " + str(E).replace('[','').replace(']','')
sys.stdout.flush()
i = 0
for ac in self.rotorcrafts:
msg = PprzMessage("datalink", "DESIRED_SETPOINT")
msg['ac_id'] = ac.id
msg['flag'] = 0 # full 3D not supported yet
msg['ux'] = U[i]
msg['uy'] = U[i+1]
msg['uz'] = self.altitude
self._interface.send(msg)
i = i+2
def run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
for rc in self.rotorcrafts:
rc.timeout = rc.timeout + self.step
# Run the formation algorithm
if self.enabled:
self.formation()
except KeyboardInterrupt:
self.stop()
class DistanceCounterFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "INS":
self.msg_count = self.msg_count + 1
newx = float(msg.get_field(0)) / 256.0
newy = float(msg.get_field(1)) / 256.0
moved = ((newx - self.ins_msg_x) ** 2 + (newy - self.ins_msg_y) ** 2)
if self.init == 0:
self.init = 1
else:
self.distance = self.distance + math.sqrt(moved)
self.ins_msg_x = newx
self.ins_msg_y = newy
self.ins_msg_z = msg.get_field(2)
# graphical update
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
# Paint Area
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
font = wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("INS Packets:" + str(self.msg_count),2,2)
dc.DrawText("Data: " + str(self.ins_msg_x) + ", " + str(self.ins_msg_y) + ", " + str(self.ins_msg_z) + ".",2,22)
dc.DrawText("Distance: " + str(round(float(self.distance)/1.0,2)) + " m",2,22+20)
def __init__(self, _settings):
# Command line arguments
self.settings = _settings
# Statistics
self.data = { 'packets': 0, 'bytes': 0}
self.w = WIDTH
self.h = WIDTH
# Frame
wx.Frame.__init__(self, id=-1, parent=None, name=u'Distance Counter',
size=wx.Size(self.w, self.h), title=u'Distance Counter')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
# IVY
self.interface = IvyMessagesInterface("DistanceCounter")
self.interface.subscribe(self.message_recv)
self.msg_count = 0
self.distance = 0
self.ins_msg_x = 0
self.ins_msg_y = 0
self.ins_msg_z = 0
self.init = 0
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class OpenSkyTraffic(object):
def __init__(self, period=10., margin=1., timeout=60., verbose=False):
self.period = period
self.margin = margin
self.timeout = timeout
self.last_receive = time()
self.verbose = verbose
self._interface = IvyMessagesInterface("OpenSkyTraffic")
self._opensky = OpenSkyApi()
self.ac_list = {}
self.intruder_list = {}
self.running = False
if self.verbose:
print("Starting opensky interface...")
# subscribe to FLIGHT_PARAM ground message
self._interface.subscribe(self.update_ac, PprzMessage("ground", "FLIGHT_PARAM"))
def stop(self):
if self.verbose:
print("Shutting down opensky interface...")
self.running = False
if self._interface is not None:
self._interface.shutdown()
def __del__(self):
self.stop()
def update_ac(self, ac_id, msg):
# update A/C info
self.last_receive = time()
self.ac_list[ac_id] = (self.last_receive, float(msg['lat']), float(msg['long']))
def filter_ac(self):
# purge timeout A/C
timeout = time() - self.timeout
for ac, (t, lat, lon) in self.ac_list.items():
if t < timeout:
del self.ac_list[ac]
def compute_bbox_list(self):
bbox = []
# for now assume that all UAVs are close enough, so merge all boxes into one
# future improvement could handle groups of UAVs far appart
lat_min, lat_max, lon_min, lon_max = None, None, None, None
for ac, (t, lat, lon) in self.ac_list.items():
if lat_min is None:
lat_min = lat
lat_max = lat
lon_min = lon
lon_max = lon
else:
lat_min = min(lat, lat_min)
lat_max = max(lat, lat_max)
lon_min = min(lon, lon_min)
lon_max = max(lon, lon_max)
if lat_min is not None:
bbox.append((lat_min-self.margin, lat_max+self.margin, lon_min-self.margin, lon_max+self.margin))
return bbox
def get_intruders(self):
self.filter_ac()
bbox = self.compute_bbox_list()
# request surounding aircraft to opensky-network
self.intruder_list = {}
for bb in bbox:
states = self._opensky.get_states(bbox=bb)
if states is not None:
for s in states.states:
self.intruder_list[s.callsign] = s
def send_intruder_msgs(self):
self.get_intruders()
def val_or_default(val, default):
if val is None:
return default
else:
return val
for i in self.intruder_list:
intruder = self.intruder_list[i]
if intruder.callsign is not None and len(intruder.callsign) > 0:
msg = PprzMessage("ground", "INTRUDER")
msg['id'] = intruder.icao24
msg['name'] = intruder.callsign.replace(" ", "")
msg['lat'] = int(intruder.latitude * 1e7)
msg['lon'] = int(intruder.longitude * 1e7)
msg['alt'] = int(val_or_default(intruder.geo_altitude, 0.) * 1000.)
msg['course'] = val_or_default(intruder.heading, 0.)
msg['speed'] = val_or_default(intruder.velocity, 0.)
msg['climb'] = val_or_default(intruder.vertical_rate, 0.)
msg['itow'] = intruder.time_position
if self.verbose:
print(msg)
self._interface.send(msg)
def run(self):
try:
self.running = True
t = 0.
while self.running:
t = t + 0.1 # increment timer until next update time is reach
if t > self.period:
self.send_intruder_msgs()
t = 0.
sleep(0.1) # wait a short time
except KeyboardInterrupt:
self.stop()
except Exception as e:
print("Failing with: "+str(e))
self.stop()
class PayloadForwarderFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "PAYLOAD":
# convert text to binary
pld = msg.get_field(0).split(",")
b = []
for p in pld:
b.append(int(p))
# forward over UDP
self.data['packets'] = self.data['packets'] + 1
self.data['bytes'] = self.data['bytes'] + len(b)
self.sock.sendto(bytearray(b), (self.settings.ip, self.settings.port))
# send to decoder
self.decoder.add_payload(b)
# graphical update
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
# Paint Area
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
font = wx.Font(11, wx.ROMAN, wx.BOLD, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("UDP: " + self.settings.ip + ":" + str(self.settings.port),2,2)
dc.DrawText("Data: " + str(self.data['packets']) + " packets, " + str(round(float(self.data['bytes'])/1024.0,2)) + "kb)",2,22)
dc.DrawText("Decoder: " + self.settings.decoder ,2,42)
# Payload visual representation
self.decoder.draw(dc, 2, 62)
def __init__(self, _settings):
# Command line arguments
self.settings = _settings
# Statistics
self.data = { 'packets': 0, 'bytes': 0}
# Decoder
if (self.settings.decoder is 'jpeg100'):
self.decoder = jpeg100_decoder.ThumbNailFromPayload()
else:
self.decoder = MinimalDecoder()
self.w = WIDTH
self.h = WIDTH
# Socket
self.sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
# Frame
wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding',
size=wx.Size(self.w, self.h), title=u'Payload Forwarding')
ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/payload.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
# IVY
self.interface = IvyMessagesInterface("PayloadForwarder")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class DroneControler(object):
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 stop(self):
# Stop IVY interface
print("Stopping Ivy interface")
if self._interface is not None:
self._interface.shutdown()
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 envoi_cmd_guidedSetPointNED(self):
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.drone.id
msg['flags'] = 0b100011 # x and y (position) as offset coordinate in pseudoBody frame
phi, theta, psi = cr.getEulerAngles(self.drone.quaternions)
Xtag_pseudoBody = cr.cam2pseudoBody(self.drone.cam.tag.X, phi, theta)
msg['x'] = Xtag_pseudoBody[0] / 2.3
msg['y'] = Xtag_pseudoBody[1] / 2.3
msg['z'] = -1.3 # COmmande en NED so the z axis is oriented facing down
msg['yaw'] = 0
print(msg)
self._interface.send_raw_datalink(msg)
def gen_commande(self, x, y, z, psi):
"""Entree : coordonnes x,y,z du drone dans le repere pseudo drone
Sortie : Commande phi, theta, psi,V_z"""
pidx = pid.PID(Px, Ix, Dx)
pidy = pid.PID(Py, Iy, Dy)
phi = pidx.update(x)
theta = pidy.update(y)
psi = getEulerAngles(self.drone.quaternions)[2] - getEulerAngles(
self.plateform.quaternions)[2]
V_z = pidz.update(z)
return phi, theta, psi, V_z
def update(self):
[self.drone.U[0], self.drone.U[1], self.drone.U[2],
self.drone.U[3]] = self.gen_commande(self.drone.X[0], self.drone.X[1],
self.drone.X[2])
self.envoi_cmd(phi, theta, psi, throttle)
def run(self):
while (True):
self.update()
class PprzConnect(object):
"""
Main class to handle the initialization process with the server
in order to retrieve the configuration of the known aircraft
and update for the new ones
"""
def __init__(self, notify=None, ivy=None, verbose=False):
"""
Init function
Create an ivy interface if not provided and request for all aircraft
:param notify: callback function called on new aircraft, takes a PprzConfig as parameter
:param ivy: ivy interface to contact the server, if None a new one will be created
:param verbose: display debug information
"""
self.verbose = verbose
self._notify = notify
self._req_idx = 0
self._conf_list_by_name = {}
self._conf_list_by_id = {}
if ivy is None:
self._ivy = IvyMessagesInterface("PprzConnect")
else:
self._ivy = ivy
sleep(0.1)
self.get_aircrafts()
def __del__(self):
self.shutdown()
def shutdown(self):
"""
Shutdown function
Should be called before leaving if the ivy interface is not closed elsewhere
"""
if self._ivy is not None:
if self.verbose:
print("Shutting down ivy interface...")
self._ivy.shutdown()
self._ivy = None
def conf_by_name(self, ac_name=None):
"""
Get a conf by its name
:param ac_name: aircraft name, if None the complete dict is returned
:type ac_name: str
"""
if ac_name is not None:
return self._conf_list_by_name[ac_name]
else:
return self._conf_list_by_name
def conf_by_id(self, ac_id=None):
"""
Get a conf by its ID
:param ac_id: aircraft id, if None the complete dict is returned
:type ac_id: str
"""
if ac_id is not None:
return self._conf_list_by_id[ac_id]
else:
return self._conf_list_by_id
@property
def ivy(self):
"""
Getter function for the ivy interface
"""
return self._ivy
def _get_req_id(self):
req_id = '{}_{}'.format(getpid(), self._req_idx)
self._req_idx += 1
return req_id
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()))
#self._ivy.send(req_msg)
def get_aircrafts(self):
"""
request all aircrafts IDs from a runing server
and new aircraft when they appear
"""
def aircrafts_cb(sender, msg):
ac_list = msg['ac_list']
for ac_id in ac_list:
self.get_config(ac_id)
#ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
if self.verbose:
print("aircrafts: {}".format(ac_list))
self._message_req("AIRCRAFTS", aircrafts_cb)
def new_ac_cb(sender, msg):
ac_id = msg['ac_id']
self.get_config(ac_id)
if self.verbose:
print("new aircraft: {}".format(ac_id))
self._ivy.subscribe(new_ac_cb, PprzMessage('ground', 'NEW_AIRCRAFT'))
def get_config(self, ac_id):
"""
Requsest a config from the server for a given ID
:param ac_id: aircraft ID
:type ac_id: str
"""
def conf_cb(sender, msg):
conf = PprzConfig(msg['ac_id'], msg['ac_name'], msg['airframe'],
msg['flight_plan'], msg['settings'],
msg['radio'], msg['default_gui_color'])
self._conf_list_by_name[conf.name] = conf
self._conf_list_by_id[int(conf.id)] = conf
if self._notify is not None:
self._notify(conf) # user defined general callback
if self.verbose:
print(conf)
self._message_req("CONFIG", conf_cb, [ac_id])
class FormationControl:
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_degrees'])*np.pi/180
self.k = np.array(self.config['gain'])
self.radius = np.array(self.config['desired_stationary_radius_meters'])
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 __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
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 run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
# Run the formation algorithm
self.circular_formation()
except KeyboardInterrupt:
self.stop()
class FormationControl:
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 __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
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 run(self):
try:
# The main loop
while True:
# TODO: make better frequency managing
sleep(self.step)
# Run the formation algorithm
self.circular_formation()
except KeyboardInterrupt:
self.stop()
class PayloadForwarderFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "PAYLOAD":
# convert text to binary
pld = msg.get_field(0).split(",")
b = []
for p in pld:
b.append(int(p))
# forward over UDP
self.data['packets'] = self.data['packets'] + 1
self.data['bytes'] = self.data['bytes'] + len(b)
self.sock.sendto(bytearray(b), (self.settings.ip, self.settings.port))
# send to decoder
self.decoder.add_payload(b)
# graphical update
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
# Paint Area
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("UDP: " + self.settings.ip + ":" + str(self.settings.port),2,2)
dc.DrawText("Data: " + str(self.data['packets']) + " packets, " + str(round(float(self.data['bytes'])/1024.0,2)) + "kb)",2,22)
dc.DrawText("Decoder: " + self.settings.decoder ,2,42)
# Payload visual representation
self.decoder.draw(dc, 2, 62)
def __init__(self, _settings):
# Command line arguments
self.settings = _settings
# Statistics
self.data = { 'packets': 0, 'bytes': 0}
# Decoder
if (self.settings.decoder == 'jpeg100'):
self.decoder = jpeg100_decoder.ThumbNailFromPayload()
else:
self.decoder = MinimalDecoder()
self.w = WIDTH
self.h = WIDTH
# Socket
self.sock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
# Frame
wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding',
size=wx.Size(self.w, self.h), title=u'Payload Forwarding')
ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/payload.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
# IVY
self.interface = IvyMessagesInterface("PayloadForwarder")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class MagCalibrator(object):
def __init__(self, plot_results=True, verbose=False):
self._interface = IvyMessagesInterface("calib_mag")
self.plotter = MagPlot()
self.data = []
self.flt_meas = []
self.p0 = np.array([0, 0, 0, 0, 0, 0])
self.optimization_done = False
self.plot_results = plot_results
def start_collect(self):
self._interface.subscribe(self.message_recv, "(.*IMU_MAG_RAW.*)")
def stop_collect(self):
self._interface.unsubscribe_all()
def message_recv(self, ac_id, msg):
self.data.append(np.array([int(v) for v in msg.fieldvalues]))
if self.plot_results:
self.plotter.add_data((map(int, msg.fieldvalues)))
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def calc_min_max_guess(self):
if len(self.data) > 3:
# filter out noisy measurements?
self.flt_meas = np.array(self.data)
self.p0 = calibration_utils.get_min_max_guess(self.flt_meas, 1.0)
def print_min_max_guess(self):
self.calc_min_max_guess()
if self.data:
print("Current guess from %d measurements: neutral [%d, %d, %d], scale [%.3f, %.3f, %.3f]" % (len(self.flt_meas),
int(round(self.p0[0])), int(round(self.p0[1])), int(round(self.p0[2])),
self.p0[3]*2**11, self.p0[4]*2**11, self.p0[5]*2**11))
def calibrate(self):
self.calc_min_max_guess()
if len(self.flt_meas) < 10:
logging.warning("Not enough measurements")
return
cp0, np0 = calibration_utils.scale_measurements(self.flt_meas, self.p0)
logging.info("initial guess : avg "+str(np0.mean())+" std "+str(np0.std()))
calibration_utils.print_xml(self.p0, "MAG", 11)
def err_func(p, meas, y):
cp, np = calibration_utils.scale_measurements(meas, p)
err = y * scipy.ones(len(meas)) - np
return err
p1, cov, info, msg, success = optimize.leastsq(err_func, self.p0[:], args=(self.flt_meas, 1.0), full_output=1)
self.optimization_done = success in [1, 2, 3, 4]
if not self.optimization_done:
logging.warning("Optimization error: ", msg)
cp1, np1 = calibration_utils.scale_measurements(self.flt_meas, p1)
if self.optimization_done:
logging.info("optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
calibration_utils.print_xml(p1, "MAG", 11)
else:
logging.info("last iteration of failed optimized guess : avg " + str(np1.mean()) + " std " + str(np1.std()))
if self.plot_results:
calibration_utils.plot_results("MAG", np.array(self.data), range(len(self.data)),
self.flt_meas, cp0, np0, cp1, np1, 1.0, blocking=False)
calibration_utils.plot_mag_3d(self.flt_meas, cp1, p1)
class WindFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ROTORCRAFT_FP":
self.ground_gs_x[self.count_gs] = float(msg['veast']) * 0.0000019
self.ground_gs_y[self.count_gs] = float(msg['vnorth']) * 0.0000019
self.last_heading = float(msg['psi']) * 0.0139882
self.count_gs = self.count_gs + 1
if self.count_gs > MSG_BUFFER_SIZE:
self.count_gs = 0
wx.CallAfter(self.update)
elif msg.name == "AIR_DATA":
self.airspeed[self.count_as] = float(msg['airspeed'])
self.heading[self.count_as] = self.last_heading * math.pi / 180.0
self.count_as = self.count_as + 1
if self.count_as > MSG_BUFFER_SIZE:
self.count_as = 0
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w))
self.cfg.Write("height", str(self.h))
self.Refresh()
def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x))
self.cfg.Write("top", str(self.y))
def OnClickD(self, event):
self.click_on = 1
def OnClickU(self, event):
self.click_on = 0
def OnClickM(self, event):
if self.click_on == 1:
m = event.GetPosition().Get()
self.click_x = m[0] - self.mid
self.click_y = m[1] - self.mid
self.Refresh()
def OnPaint(self, e):
w = float(self.w)
h = float(self.h)
if w / h > (WIDTH / (WIDTH + BARH)):
w = (WIDTH / (WIDTH + BARH)) * h
else:
h = ((WIDTH + BARH) / (WIDTH)) * w
bar = BARH / WIDTH * w
tdx = -5.0 / WIDTH * w
tdy = -7.0 / WIDTH * w
th = 15.0 / WIDTH * w
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
self.mid = w / 2
diameter = w / 2
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
# Speed circles
for v in range(0, 40, 5):
dc.DrawCircle(self.mid, self.mid, diameter * v / MAX_AIRSPEED)
font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL,
wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("N", self.mid + tdx, 2)
dc.DrawText("S", self.mid + tdx, w - 17)
dc.DrawText("E", w - 15, w / 2 + tdy)
dc.DrawText("W", 2, w / 2 + tdy)
# Ground Speed
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 255), wx.SOLID))
for i in range(0, MSG_BUFFER_SIZE):
gx = self.ground_gs_x[i]
gy = self.ground_gs_y[i]
dc.DrawCircle(
int(gx * diameter / MAX_AIRSPEED + self.mid + self.click_x),
int(gy * diameter / MAX_AIRSPEED + self.mid + self.click_y), 2)
# Airspeed in function of heading
dc.SetBrush(wx.Brush(wx.Colour(255, 0, 0), wx.SOLID))
for i in range(0, MSG_BUFFER_SIZE):
gx = self.airspeed[i] * math.cos(self.heading[i])
gy = self.airspeed[i] * math.sin(self.heading[i])
dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid),
int(gy * diameter / MAX_AIRSPEED + self.mid), 2)
# Result
font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText(
"#" + str(self.count_gs) + ", " + str(self.count_as) + " | " +
str(self.click_x) + "-" + str(self.click_y), 0, h - 14)
windspeed = math.sqrt(self.click_x * +self.click_x + +self.click_y *
+self.click_y) / diameter * MAX_AIRSPEED
windheading = math.atan2(self.click_x, -self.click_y) * 180 / math.pi
fontsize = int(16.0 / WIDTH * w)
font = wx.Font(fontsize, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText(
"Wind = " + str(round(windspeed, 1)) + " m/s from " +
str(round(windheading, 0)), 0, w)
def __init__(self):
# own data
self.count_gs = 0
self.ground_gs_x = [0] * MSG_BUFFER_SIZE
self.ground_gs_y = [0] * MSG_BUFFER_SIZE
self.count_as = 0
self.last_heading = 0
self.airspeed = [0] * MSG_BUFFER_SIZE
self.heading = [0] * MSG_BUFFER_SIZE
# Click
self.click_x = 0
self.click_y = 0
self.click_on = 0
# Window
self.w = WIDTH
self.h = WIDTH + BARH
self.cfg = wx.Config('wind_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))
self.mid = self.w / 2
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'WindFrame',
size=wx.Size(self.w, self.h),
title=u'Wind Tool')
if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD)
self.Bind(wx.EVT_MOTION, self.OnClickM)
self.Bind(wx.EVT_LEFT_UP, self.OnClickU)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png",
wx.BITMAP_TYPE_PNG)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("windframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class AtcFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "INS_REF":
self.qfe = round(float(msg['baro_qfe']) / 100.0,1)
wx.CallAfter(self.update)
elif msg.name =="ROTORCRAFT_FP_MIN":
self.gspeed = round(float(msg['gspeed']) / 100.0 * 3.6 / 1.852,1)
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
wx.CallAfter(self.update)
elif msg.name =="ROTORCRAFT_FP":
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
wx.CallAfter(self.update)
elif msg.name =="AIR_DATA":
self.airspeed = round(float(msg['airspeed']) * 3.6 / 1.852,1)
self.qnh = round(float(msg['qnh']),1)
self.amsl = round(float(msg['amsl_baro']) * 3.28084,1)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnPaint(self, e):
tdx = 10
tdy = 80
w = self.w
h = self.w
dc = wx.PaintDC(self)
#brush = wx.Brush("white")
#dc.SetBackground(brush)
#dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
#dc.DrawCircle(w/2,w/2,w/2-1)
font = wx.Font(40, wx.ROMAN, wx.BOLD, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("Airspeed: " + str(self.airspeed) + " kt",tdx,tdx)
dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt",tdx,tdx+tdy*1)
dc.DrawText("AMSL: " + str(self.amsl) + " ft",tdx,tdx+tdy*2)
dc.DrawText("QNH: " + str(self.qnh) + " ",tdx,tdx+tdy*3)
dc.DrawText("ALT: " + str(self.alt) + " ",tdx,tdx+tdy*4)
dc.DrawText("QFE: " + str(self.qfe) + " ",tdx,tdx+tdy*5)
#dc.DrawText("HMSL: " + str(self.hmsl) + " ft",tdx,tdx+tdy*6)
#c = wx.Colour(0,0,0)
#dc.SetBrush(wx.Brush(c, wx.SOLID))
#dc.DrawCircle(int(w/2),int(w/2),10)
def __init__(self):
self.w = 900
self.h = 700
self.airspeed = 0;
self.amsl = 0;
self.qnh = 0;
self.qfe = 0;
self.alt = 0;
#self.hmsl = 0;
self.gspeed = 0;
wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center',
size=wx.Size(self.w, self.h), title=u'ATC Center')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("ATC-Center")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class EnergyMonFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ENERGY":
self.bat = EnergyMessage(msg)
self.cell.fill_from_energy_msg(self.bat)
wx.CallAfter(self.update)
elif msg.name == "TEMP_ADC":
self.temp = TempMessage(msg)
self.cell.fill_from_temp_msg(self.temp)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def StatusBox(self, dc, nr, txt, percent, color):
if percent < 0:
percent = 0
if percent > 1:
percent = 1
boxw = self.stat
tdx = int(boxw * 10.0 / 300.0)
tdy = int(boxw * 6.0 / 300.0)
boxh = int(boxw * 40.0 / 300.0)
boxw = self.stat - 2*tdx
spacing = boxh+10
dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
dc.SetBrush(wx.Brush(wx.Colour(220,220,220)))
dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw), boxh)
if color < 0.2:
dc.SetBrush(wx.Brush(wx.Colour(250,0,0)))
elif color < 0.6:
dc.SetBrush(wx.Brush(wx.Colour(250,180,0)))
else:
dc.SetBrush(wx.Brush(wx.Colour(0,250,0)))
# dc.DrawLine(200,50,350,50)
dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw * percent), boxh)
dc.DrawText(txt,18,int(nr*spacing+tdy+tdx))
def plot_x(self, x):
return int(self.stat+self.tdx + x * (self.w-self.stat-2*self.tdx))
def plot_y(self, y):
return int(self.tdx + (1-y) * (self.h-self.tdx-self.tdx))
def plot(self, dc, i1, i2):
dc.DrawLine(self.plot_x(i1[1]/3500), self.plot_y((i1[0]-2.5)/(4.2-2.5)), self.plot_x(i2[1]/3500), self.plot_y((i2[0]-2.5)/(4.2-2.5)))
def DischargePlot(self, dc):
self.tdx = int(self.stat * 10.0 / 300.0)
dc.SetPen(wx.Pen(wx.Colour(0,0,0),1))
dc.SetBrush(wx.Brush(wx.Colour(250,250,250)))
dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0), self.w-self.stat-2*self.tdx, self.h-2*self.tdx)
for i in range(0,5):
dc.DrawLine(self.plot_x(0.0), self.plot_y(i/5.0), self.plot_x(1.0), self.plot_y(i/5.0))
for i in range(0,7):
dc.DrawLine(self.plot_x(i/7.0), self.plot_y(0), self.plot_x(i/7.0), self.plot_y(1))
dc.SetPen(wx.Pen(wx.Colour(255,180,0),4))
dc.DrawLine(self.plot_x(self.cell.model/3500), self.plot_y(0), self.plot_x(self.cell.model/3500), self.plot_y(1))
dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()), self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc()))
thickness = 3
dc.SetPen(wx.Pen(wx.Colour(0,0,0),thickness))
li = bat.batmodel[0,[0,1]]
for i in bat.batmodel[:,[0,1]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(0,0,255),thickness))
li = bat.batmodel[0,[0,2]]
for i in bat.batmodel[:,[0,2]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(0,255,0),thickness))
li = bat.batmodel[0,[0,3]]
for i in bat.batmodel[:,[0,3]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(255,255,0),thickness))
li = bat.batmodel[0,[0,4]]
for i in bat.batmodel[:,[0,4]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(255,0,0),thickness))
li = bat.batmodel[0,[0,5]]
for i in bat.batmodel[:,[0,5]]:
self.plot(dc,li,i)
li=i
def OnPaint(self, e):
# Automatic Scaling
w = self.w
h = self.h
self.stat = int(w/4)
if self.stat<100:
self.stat=100
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
fontscale = int(w * 11.0 / 800.0)
if fontscale < 6:
fontscale = 6
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
self.StatusBox(dc,0, self.cell.get_volt(), self.cell.get_volt_perc(), self.cell.get_volt_color())
self.StatusBox(dc,1, self.cell.get_current(), self.cell.get_current_perc(), self.cell.get_current_color() )
self.StatusBox(dc,2, self.cell.get_energy(), self.cell.get_energy_perc(), self.cell.get_energy_color() )
self.StatusBox(dc,3, self.cell.get_mah_from_volt(), self.cell.get_energy_perc(), self.cell.get_energy_color() )
self.StatusBox(dc,4, self.cell.get_temp(), self.cell.get_temp_perc(), self.cell.get_temp_color())
self.DischargePlot(dc)
def __init__(self):
self.w = WIDTH
self.h = WIDTH + BARH
wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame',
size=wx.Size(self.w, self.h), title=u'Energy Monitoring')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.bat = {}
self.temp = {}
self.cell = BatteryCell();
self.interface = IvyMessagesInterface("energymonframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'].index
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: '+str(bat)+' V')
self._interface.subscribe(bat_cb, regex=('(^ground ENGINE_STATUS '+str(self.ac_id)+' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
from pprzlink import messages_xml_map
try:
msgs = messages_xml_map.get_msgs('test')
except Exception as e:
print(e)
dico = messages_xml_map.message_dictionary
for msg_type in dico.keys():
for msg in dico[msg_type]:
print(msg_type, ":", msg)
ac_id = 24
ivyInterface = IvyMessagesInterface()
time.sleep(0.5)
world = None
uavid = None
def callback01(ac_id, msg, request_id):
print(request_id, msg)
def callback02(ac_id, msg):
print(msg)
ivyInterface.subscribe(callback01, '(.* WORLD_ENV_REQ .*)')
ivyInterface.subscribe(callback02, '(.* GPS .*)')
signal.signal(signal.SIGINT, lambda frame, sig: ivyInterface.stop())
class PprzConnect(object):
"""
Main class to handle the initialization process with the server
in order to retrieve the configuration of the known aircraft
and update for the new ones
"""
def __init__(self, notify=None, ivy=None, verbose=False):
"""
Init function
Create an ivy interface if not provided and request for all aircraft
:param notify: callback function called on new aircraft, takes a PprzConfig as parameter
:param ivy: ivy interface to contact the server, if None a new one will be created
:param verbose: display debug information
"""
self.verbose = verbose
self._notify = notify
self._conf_list_by_name = {}
self._conf_list_by_id = {}
if ivy is None:
self._ivy = IvyMessagesInterface("PprzConnect")
else:
self._ivy = ivy
sleep(0.1)
self.get_aircrafts()
def __del__(self):
self.shutdown()
def shutdown(self):
"""
Shutdown function
Should be called before leaving if the ivy interface is not closed elsewhere
"""
if self._ivy is not None:
if self.verbose:
print("Shutting down ivy interface...")
self._ivy.shutdown()
self._ivy = None
def conf_by_name(self, ac_name=None):
"""
Get a conf by its name
:param ac_name: aircraft name, if None the complete dict is returned
:type ac_name: str
"""
if ac_name is not None:
return self._conf_list_by_name[ac_name]
else:
return self._conf_list_by_name
def conf_by_id(self, ac_id=None):
"""
Get a conf by its ID
:param ac_id: aircraft id, if None the complete dict is returned
:type ac_id: str
"""
if ac_id is not None:
return self._conf_list_by_id[ac_id]
else:
return self._conf_list_by_id
@property
def ivy(self):
"""
Getter function for the ivy interface
"""
return self._ivy
def get_aircrafts(self):
"""
request all aircrafts IDs from a runing server
and new aircraft when they appear
"""
def aircrafts_cb(sender, msg):
ac_list = msg['ac_list']
for ac_id in ac_list:
self.get_config(ac_id)
#ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
if self.verbose:
print("aircrafts: {}".format(ac_list))
self._ivy.send_request('ground', "AIRCRAFTS", aircrafts_cb)
def new_ac_cb(sender, msg):
ac_id = msg['ac_id']
self.get_config(ac_id)
if self.verbose:
print("new aircraft: {}".format(ac_id))
self._ivy.subscribe(new_ac_cb,PprzMessage('ground','NEW_AIRCRAFT'))
def get_config(self, ac_id):
"""
Requsest a config from the server for a given ID
:param ac_id: aircraft ID
:type ac_id: str
"""
def conf_cb(sender, msg):
conf = PprzConfig(msg['ac_id'], msg['ac_name'], msg['airframe'],
msg['flight_plan'], msg['settings'], msg['radio'],
msg['default_gui_color'])
self._conf_list_by_name[conf.name] = conf
self._conf_list_by_id[int(conf.id)] = conf
if self._notify is not None:
self._notify(conf) # user defined general callback
if self.verbose:
print(conf)
self._ivy.send_request('ground', "CONFIG", conf_cb, ac_id=ac_id)
"--verbose",
action="store_true",
help="verbose mode")
try:
# --- Startup state initialization block
args = parser.parse_args()
static_init_configuration_data()
static_init_client_configuration_data(args.file)
if args.verbose:
print_aircraft_data()
if args.generate:
template_configuration()
sys.exit(0)
if args.subscribe:
ivy_interface.subscribe(callback_aircraft_messages)
ivy_interface.start()
# Handle misc. command line arguments
if args.verbose:
verbose = args.verbose
if args.curl:
curl = args.curl
print_curl_header(args.ip, args.port)
# --- Main loop
# Supply flask the appropriate ip address and port for the server
server_host = args.ip # Store for use in htlm template generation
server_port = args.port # Store for use in htlm template generation
app.run(host=args.ip, port=args.port, threaded=True)
class GVFFrame(wx.Frame):
def __init__(self, ac_id):
wx.Frame.__init__(self, id=-1, parent=None, \
name=u'GVF', size=wx.Size(WIDTH, HEIGHT), \
style=wx.DEFAULT_FRAME_STYLE, title=u'Guidance Vector Field')
# Vehicle variables
self.ac_id = ac_id
self.course = 0
self.yaw = 0
self.XY = np.array([0, 0])
# Desired trajectory
self.timer_traj = 0 # We do not update the traj every time we receive a msg
self.timer_traj_lim = 7 # (7+1) * 0.25secs
self.s = 0
self.kn = 0
self.ke = 0
self.map_gvf = map2d(np.array([0, 0]), 150000)
self.traj = None
# Frame
self.canvas = FigureCanvas(self, -1, self.map_gvf.fig)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.redraw_timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.OnRedrawTimer, self.redraw_timer)
self.redraw_timer.Start(100)
# Ivy
self.interface = IvyMessagesInterface("GVF")
self.interface.subscribe(self.message_recv)
settings = PaparazziACSettings(ac_id)
self.ke_index = None
self.kn_index = None
self.indexes_are_good = 0
self.list_of_indexes = ['gvf_ke', 'gvf_kn']
for setting_ in self.list_of_indexes:
try:
index = settings.name_lookup[setting_].index
if setting_ == 'gvf_ke':
self.ke_index = index
if setting_ == 'gvf_kn':
self.kn_index = index
self.indexes_are_good = self.indexes_are_good + 1
except Exception as e:
print(e)
print(setting_ + " setting not found, \
have you forgotten gvf.xml in your settings?")
def message_recv(self, ac_id, msg):
if int(ac_id) == self.ac_id:
if msg.name == 'GPS':
self.course = int(msg.get_field(3)) * np.pi / 1800
if msg.name == 'NAVIGATION':
self.XY[0] = float(msg.get_field(2))
self.XY[1] = float(msg.get_field(3))
if msg.name == 'ATTITUDE':
self.yaw = float(msg.get_field(1))
if msg.name == 'DL_VALUE' and \
self.indexes_are_good == len(self.list_of_indexes):
if int(msg.get_field(0)) == int(self.ke_index):
self.ke = float(msg.get_field(1))
if self.traj is not None:
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.kn, self.ke)
if int(msg.get_field(0)) == int(self.kn_index):
self.kn = float(msg.get_field(1))
if self.traj is not None:
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.kn, self.ke)
if msg.name == 'GVF':
self.gvf_error = float(msg.get_field(0))
# Straight line
if int(msg.get_field(1)) == 0 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
param = [float(x) for x in msg.get_field(3).split(',')]
a = param[0]
b = param[1]
c = param[2]
self.traj = traj_line(np.array([-100, 100]), a, b, c)
self.traj.vector_field(self.traj.XYoff, self.map_gvf.area, \
self.s, self.kn, self.ke)
# Ellipse
if int(msg.get_field(1)) == 1 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
param = [float(x) for x in msg.get_field(3).split(',')]
ex = param[0]
ey = param[1]
ea = param[2]
eb = param[3]
ealpha = param[4]
self.traj = traj_ellipse(np.array([ex, ey]), ealpha, ea,
eb)
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.kn, self.ke)
# Sin
if int(msg.get_field(1)) == 2 \
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2))
param = [float(x) for x in msg.get_field(3).split(',')]
a = param[0]
b = param[1]
alpha = param[2]
w = param[3]
off = param[4]
A = param[5]
self.traj = traj_sin(np.array([-100, 100]), a, b, alpha, \
w, off, A)
self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.kn, self.ke)
self.timer_traj = self.timer_traj + 1
if self.timer_traj > self.timer_traj_lim:
self.timer_traj = 0
def draw_gvf(self, XY, yaw, course):
if self.traj is not None:
self.map_gvf.draw(XY, yaw, course, self.traj)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
def OnRedrawTimer(self, event):
self.draw_gvf(self.XY, self.yaw, self.course)
self.canvas.draw()
class WindFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ROTORCRAFT_FP":
self.ground_gs_x[self.count_gs] = float(msg['veast']) * 0.0000019
self.ground_gs_y[self.count_gs] = float(msg['vnorth']) * 0.0000019
self.last_heading = float(msg['psi']) * 0.0139882
self.count_gs = self.count_gs + 1
if self.count_gs > MSG_BUFFER_SIZE:
self.count_gs = 0
wx.CallAfter(self.update)
elif msg.name =="AIR_DATA":
self.airspeed[self.count_as] = float(msg['airspeed'])
self.heading[self.count_as] = self.last_heading * math.pi / 180.0
self.count_as = self.count_as + 1
if self.count_as > MSG_BUFFER_SIZE:
self.count_as = 0
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w));
self.cfg.Write("height", str(self.h));
self.Refresh()
def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x));
self.cfg.Write("top", str(self.y));
def OnClickD(self,event):
self.click_on = 1
def OnClickU(self,event):
self.click_on = 0
def OnClickM(self,event):
if self.click_on == 1:
m = event.GetPosition().Get()
self.click_x = m[0] - self.mid
self.click_y = m[1] - self.mid
self.Refresh()
def OnPaint(self, e):
w = float(self.w)
h = float(self.h)
if w/h > (WIDTH / (WIDTH+BARH)):
w = (WIDTH / (WIDTH+BARH)) * h
else:
h = ((WIDTH+BARH) / (WIDTH)) * w
bar = BARH / WIDTH * w
tdx = -5.0 / WIDTH * w
tdy = -7.0 / WIDTH * w
th = 15.0 / WIDTH * w
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
self.mid = w/2
diameter = w/2
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
# Speed circles
for v in range(0,40,5):
dc.DrawCircle(self.mid, self.mid, diameter * v / MAX_AIRSPEED )
font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("N", self.mid + tdx, 2)
dc.DrawText("S", self.mid + tdx, w - 17)
dc.DrawText("E", w - 15, w / 2 + tdy)
dc.DrawText("W", 2, w / 2 + tdy)
# Ground Speed
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 255), wx.SOLID))
for i in range(0,MSG_BUFFER_SIZE):
gx = self.ground_gs_x[i]
gy = self.ground_gs_y[i]
dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid + self.click_x), int(gy * diameter / MAX_AIRSPEED + self.mid + self.click_y), 2)
# Airspeed in function of heading
dc.SetBrush(wx.Brush(wx.Colour(255, 0, 0), wx.SOLID))
for i in range(0,MSG_BUFFER_SIZE):
gx = self.airspeed[i] * math.cos(self.heading[i])
gy = self.airspeed[i] * math.sin(self.heading[i])
dc.DrawCircle(int(gx * diameter / MAX_AIRSPEED + self.mid), int(gy * diameter / MAX_AIRSPEED + self.mid), 2)
# Result
font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("#" + str(self.count_gs) + ", " + str(self.count_as) + " | " + str(self.click_x) + "-" + str(self.click_y), 0, h - 14)
windspeed = math.sqrt(self.click_x * +self.click_x + +self.click_y * +self.click_y) / diameter * MAX_AIRSPEED
windheading = math.atan2(self.click_x,-self.click_y) * 180 / math.pi
fontsize = int(16.0 / WIDTH * w)
font = wx.Font(fontsize, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("Wind = " + str(round(windspeed,1)) + " m/s from " + str(round(windheading, 0)), 0, w )
def __init__(self):
# own data
self.count_gs = 0
self.ground_gs_x = [0] * MSG_BUFFER_SIZE
self.ground_gs_y = [0] * MSG_BUFFER_SIZE
self.count_as = 0
self.last_heading = 0;
self.airspeed = [0] * MSG_BUFFER_SIZE
self.heading = [0] * MSG_BUFFER_SIZE
# Click
self.click_x = 0
self.click_y = 0
self.click_on = 0
# Window
self.w = WIDTH
self.h = WIDTH + BARH
self.cfg = wx.Config('wind_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))
self.mid = self.w/2
wx.Frame.__init__(self, id=-1, parent=None, name=u'WindFrame',
size=wx.Size(self.w, self.h), title=u'Wind Tool')
if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.Bind(wx.EVT_LEFT_DOWN, self.OnClickD)
self.Bind(wx.EVT_MOTION, self.OnClickM)
self.Bind(wx.EVT_LEFT_UP, self.OnClickU)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/wind/wind.png", wx.BITMAP_TYPE_PNG)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("windframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class EnergyMonFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "ENERGY":
self.bat = EnergyMessage(msg)
self.cell.fill_from_energy_msg(self.bat)
wx.CallAfter(self.update)
elif msg.name == "TEMP_ADC":
self.temp = TempMessage(msg)
self.cell.fill_from_temp_msg(self.temp)
wx.CallAfter(self.update)
elif msg.name == "AIR_DATA":
self.air_data = AirDataMessage(msg)
self.energy_prediction.fill_from_air_data_msg(self.air_data)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w));
self.cfg.Write("height", str(self.h));
self.Refresh()
def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x));
self.cfg.Write("top", str(self.y));
def StatusBox(self, dc, nr, txt, percent, color):
if percent < 0:
percent = 0
if percent > 1:
percent = 1
boxw = self.stat
tdx = int(boxw * 10.0 / 300.0)
tdy = int(boxw * 6.0 / 300.0)
boxh = int(boxw * 40.0 / 300.0)
boxw = self.stat - 2*tdx
spacing = boxh+10
dc.SetPen(wx.Pen(wx.Colour(0,0,0)))
dc.SetBrush(wx.Brush(wx.Colour(220,220,220)))
dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw), boxh)
dc.SetTextForeground(wx.Colour(0, 0, 0))
if color < 0.2:
dc.SetTextForeground(wx.Colour(255, 255, 255))
dc.SetBrush(wx.Brush(wx.Colour(250,0,0)))
elif color < 0.6:
dc.SetBrush(wx.Brush(wx.Colour(250,180,0)))
else:
dc.SetBrush(wx.Brush(wx.Colour(0,250,0)))
# dc.DrawLine(200,50,350,50)
dc.DrawRectangle(tdx, int(nr*spacing+tdx), int(boxw * percent), boxh)
dc.DrawText(txt,18,int(nr*spacing+tdy+tdx))
def plot_x(self, x):
return int(self.stat+self.tdx + x * (self.w-self.stat-2*self.tdx))
def plot_y(self, y):
return int(self.tdx + (1-y) * (self.h-self.tdx-self.tdx))
def plot(self, dc, i1, i2):
dc.DrawLine(self.plot_x(i1[1]/3500), self.plot_y((i1[0]-2.5)/(4.2-2.5)), self.plot_x(i2[1]/3500), self.plot_y((i2[0]-2.5)/(4.2-2.5)))
def DischargePlot(self, dc):
self.tdx = int(self.stat * 10.0 / 300.0)
dc.SetPen(wx.Pen(wx.Colour(0,0,0),1))
dc.SetBrush(wx.Brush(wx.Colour(250,250,250)))
dc.DrawRectangle(self.plot_x(0.0), self.plot_y(1.0), self.w-self.stat-2*self.tdx, self.h-2*self.tdx)
for i in range(0,6):
dc.DrawLine(self.plot_x(0.0), self.plot_y(i/6.0), self.plot_x(1.0), self.plot_y(i/6.0))
for i in range(0,7):
dc.DrawLine(self.plot_x(i/7.0), self.plot_y(0), self.plot_x(i/7.0), self.plot_y(1))
dc.SetPen(wx.Pen(wx.Colour(0,0,0),4))
dc.DrawLine(self.plot_x(self.cell.model/3500), self.plot_y(0), self.plot_x(self.cell.model/3500), self.plot_y(1))
dc.DrawLine(self.plot_x(0.0), self.plot_y(self.cell.get_volt_perc()), self.plot_x(1.0), self.plot_y(self.cell.get_volt_perc()))
# Draw maximum charge point
dc.SetPen(wx.Pen(wx.Colour(0,0,255),2))
dc.DrawLine(self.plot_x(self.energy_prediction.get_max_hover_charge() / 3500), self.plot_y(0), self.plot_x(self.energy_prediction.get_max_hover_charge() / 3500), self.plot_y(1))
dc.DrawLine(self.plot_x(1415. / 3500), self.plot_y(0), self.plot_x(1415. / 3500), self.plot_y(1)) # Competition latest land at joe (18km/h0degwind, 25m/s)
font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
for i in range(0,3500,500):
dc.DrawText(str(i) + "mAh", self.plot_x(float(i)/3500.0), self.plot_y(1.0))
for i in range(25,43,3):
dc.DrawText(str(round(i/10.0,1)) + "V", self.plot_x(0), self.plot_y(self.cell.get_volt_percent(float(i/10.0))))
thickness = 3
dc.SetPen(wx.Pen(wx.Colour(0,0,0),thickness))
li = bat.batmodel[0,[0,1]]
for i in bat.batmodel[:,[0,1]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(0,0,255),thickness))
li = bat.batmodel[0,[0,2]]
for i in bat.batmodel[:,[0,2]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(0,255,0),thickness))
li = bat.batmodel[0,[0,3]]
for i in bat.batmodel[:,[0,3]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(255,255,0),thickness))
li = bat.batmodel[0,[0,4]]
for i in bat.batmodel[:,[0,4]]:
self.plot(dc,li,i)
li=i
dc.SetPen(wx.Pen(wx.Colour(255,0,0),thickness))
li = bat.batmodel[0,[0,5]]
for i in bat.batmodel[:,[0,5]]:
self.plot(dc,li,i)
li=i
def OnPaint(self, e):
# Automatic Scaling
w = self.w
h = self.h
self.stat = int(w/4)
if self.stat<100:
self.stat=100
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
fontscale = int(w * 11.0 / 800.0)
if fontscale < 6:
fontscale = 6
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
self.StatusBox(dc,0, self.cell.get_volt(), self.cell.get_volt_perc(), self.cell.get_volt_color())
self.StatusBox(dc,1, self.cell.get_current(), self.cell.get_current_perc(), self.cell.get_current_color() )
self.StatusBox(dc,2, self.cell.get_energy(), self.cell.get_energy_perc(), self.cell.get_energy_color() )
self.StatusBox(dc,3, self.cell.get_mah_from_volt(), self.cell.get_energy_perc(), self.cell.get_energy_color() )
self.StatusBox(dc,4, self.cell.get_temp(), self.cell.get_temp_perc(), self.cell.get_temp_color())
self.StatusBox(dc,6, self.cell.get_power_text(), self.cell.get_power_perc(), self.cell.get_power_color())
self.StatusBox(dc,7, self.energy_prediction.get_power_fraction_text(), self.energy_prediction.get_power_fraction(), self.energy_prediction.get_power_fraction_color())
self.StatusBox(dc,8, self.energy_prediction.get_hover_seconds_left_text(), self.energy_prediction.get_hover_seconds_fraction(), self.energy_prediction.get_hover_seconds_color())
self.StatusBox(dc,9, self.energy_prediction.get_fw_seconds_left_text(), self.energy_prediction.get_fw_seconds_fraction(), self.energy_prediction.get_fw_seconds_color())
self.StatusBox(dc,10, self.energy_prediction.get_fw_seconds_left_20mps_text(), self.energy_prediction.get_fw_seconds_left_20mps_fraction(), self.energy_prediction.get_fw_seconds_left_20mps_color())
self.DischargePlot(dc)
def __init__(self):
self.w = WIDTH
self.h = WIDTH + BARH
self.cfg = wx.Config('energymon_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))
wx.Frame.__init__(self, id=-1, parent=None, name=u'EnergyMonFrame',
size=wx.Size(self.w, self.h), title=u'Energy Monitoring')
if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING)
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/energy_mon/energy_mon.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.bat = {}
self.temp = {}
self.cell = BatteryCell()
self.energy_prediction = EnergyPrediction(self.cell)
self.interface = IvyMessagesInterface("energymonframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class DistanceCounterFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "INS":
self.msg_count = self.msg_count + 1
newx = float(msg.get_field(0)) / 256.0
newy = float(msg.get_field(1)) / 256.0
moved = ((newx - self.ins_msg_x)**2 + (newy - self.ins_msg_y)**2)
if self.init == 0:
self.init = 1
else:
self.distance = self.distance + math.sqrt(moved)
self.ins_msg_x = newx
self.ins_msg_y = newy
self.ins_msg_z = msg.get_field(2)
# graphical update
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
# Paint Area
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
font = wx.Font(11, wx.DEFAULT, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("INS Packets:" + str(self.msg_count), 2, 2)
dc.DrawText(
"Data: " + str(self.ins_msg_x) + ", " + str(self.ins_msg_y) +
", " + str(self.ins_msg_z) + ".", 2, 22)
dc.DrawText(
"Distance: " + str(round(float(self.distance) / 1.0, 2)) + " m", 2,
22 + 20)
def __init__(self, _settings):
# Command line arguments
self.settings = _settings
# Statistics
self.data = {'packets': 0, 'bytes': 0}
self.w = WIDTH
self.h = WIDTH
# Frame
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'Distance Counter',
size=wx.Size(self.w, self.h),
title=u'Distance Counter')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
# IVY
self.interface = IvyMessagesInterface("DistanceCounter")
self.interface.subscribe(self.message_recv)
self.msg_count = 0
self.distance = 0
self.ins_msg_x = 0
self.ins_msg_y = 0
self.ins_msg_z = 0
self.init = 0
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.ap_mode = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.ap_mode = settings.name_lookup['mode'] # try classic name
except Exception as e:
try:
self.ap_mode = settings.name_lookup[
'ap'] # in case it is a generated autopilot
except Exception as e:
print(e)
print("ap_mode setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("gb2ivy")
def shutdown(self):
if self._interface is not None:
print("Shutting down ivy interface...")
self._interface.shutdown()
self._interface = None
def __del__(self):
self.shutdown()
def bind_flight_param(self, send_cb):
def bat_cb(ac_id, msg):
bat = float(msg['bat'])
# should not be more that 18 characters
send_cb('bat: ' + str(bat) + ' V')
self._interface.subscribe(bat_cb,
regex=('(^ground ENGINE_STATUS ' +
str(self.ac_id) + ' .*)'))
def set_guided_mode(self):
"""
change mode to GUIDED.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName(
'Guided') # AP_MODE_GUIDED
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'GUIDED') # AP_MODE_GUIDED
except ValueError:
msg['value'] = 19 # fallback to fixed index
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change mode to NAV.
"""
if self.ap_mode is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.ap_mode.index
try:
msg['value'] = self.ap_mode.ValueFromName('Nav') # AP_MODE_NAV
except ValueError:
try:
msg['value'] = self.ap_mode.ValueFromName(
'NAV') # AP_MODE_NAV
except ValueError:
msg['value'] = 13 # fallback to fixed index
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def move_at_body_vel(self, forward=0.0, right=0.0, down=0.0, yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0xE2
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
def command_callback(self, command):
"""
convert incoming command into velocity
"""
right = 0.0
forward = 0.0
down = 0.0
yaw = 0.0
command = int(command)
if command & J_RIGHT:
right += 2.0
if command & J_LEFT:
right -= 2.0
if command & J_UP:
forward += 2.0
if command & J_DOWN:
forward -= 2.0
if command & J_A:
down -= 1.0
if command & J_B:
down += 1.0
if command & J_START:
yaw += radians(20)
if command & J_SELECT:
yaw -= radians(20)
self.move_at_body_vel(forward, right, down, yaw)
class MessageInterface:
"""
MessageInterface
This is an abstraction layer used to simplify the use of paparazzi
message interface, especially for time measurement and casting of
message payload data (which sometimes stays in ascii)
"""
def prettify_message(msg):
"""
Sometimes IvyMessageInterface does not cast data to their binary types.
This function cast all fields to their binary types. (supposed to be
done by PprzMessage.payload_to_binay but does not seem to be working)
It also measure reception time.
"""
timestamp = time.time()
fieldValues = []
for fieldValue, fieldType in zip(msg.fieldvalues, msg.fieldtypes):
if "int" in fieldType:
castType = int
elif "float" in fieldType:
castType = float
elif "string" in fieldType:
castType = str
elif "char" in fieldType:
castType = int
else:
# Could not indentify type, leave field as is
fieldValues.append(fieldValue)
# Checking if is a list
if '[' in fieldType:
fieldValues.append([castType(value) for value in fieldValue])
else:
fieldValues.append(castType(fieldValue))
msg.set_values(fieldValues)
msg.timestamp = timestamp
return msg
def parse_pprz_msg(msg):
"""
Alias to IvyMessageInterface.parse_pprz_msg, but with prettify_message
called at the end to ensure all data are in binary format.
"""
class Catcher:
"""
This is a type specifically to catch result from
IvyMessageInterface.parse_pprz_msg which only outputs result via a
callback.
"""
def set_message(self, aircraftId, message):
self.message = message
self.aircraftId = str(aircraftId)
catcher = Catcher()
IvyMessagesInterface.parse_pprz_msg(catcher.set_message, msg)
return [MessageInterface.prettify_message(catcher.message),
catcher.aircraftId]
def __init__(self, ivyBusAddress=None):
if ivyBusAddress is None:
ivyBusAddress = os.getenv('IVY_BUS')
if ivyBusAddress is None:
ivyBusAddress == ""
self.messageInterface = IvyMessagesInterface(ivy_bus=ivyBusAddress)
def bind(self, callback, ivyRegex):
return self.messageInterface.subscribe(
lambda sender, msg: callback(MessageInterface.prettify_message(msg)),
ivyRegex)
def bind_raw(self, callback, ivyRegex):
return self.messageInterface.bind_raw(callback, ivyRegex)
def unbind(self, bindId):
self.messageInterface.unbind(bindId)
def send(self, msg):
return self.messageInterface.send(msg)
class SVInfoFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "SVINFO":
chn = int(msg['chn'])
self.sv[chn] = SvChannel(chn, msg)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
tdx = -5
tdy = -7
th = 15
w = self.w
h = self.w
if h < self.w + 50:
h = self.w + 50
bar = self.h - w - th - th
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
dc.DrawCircle(w / 2, w / 2, w / 2 - 1)
dc.DrawCircle(w / 2, w / 2, w / 4 - 1)
dc.DrawCircle(w / 2, w / 2, 1)
font = wx.Font(11, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("N", w / 2 + tdx, 2)
dc.DrawText("S", w / 2 + tdx, w - 17)
dc.DrawText("E", w - 15, w / 2 + tdy)
dc.DrawText("W", 2, w / 2 + tdy)
# SV
for chn in self.sv:
sv = self.sv[chn]
c = QIColour(sv.QI)
used = sv.Flags & 1
s = 7 + used * 5
el = float(sv.Elev) / 90.0 * float(w) / 2.0
az = float(sv.Azim) * math.pi / 180.0
y = float(w) / 2.0 - math.cos(az) * el
x = float(w) / 2.0 + math.sin(az) * el
dc.SetBrush(wx.Brush(c, wx.SOLID))
dc.DrawCircle(int(x), int(y), s)
font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText(str(sv.SVID), x + tdx, y + tdy)
bh = float(bar - th - th) * float(sv.CNO) / 55.0
dc.DrawRectangle(w / CHANNEL * chn + 5 * (1 - used), self.h - th - bh, w / CHANNEL - 2 - 10 * (1 - used), bh)
dc.DrawText(str(chn), w / CHANNEL * chn, self.h - th)
dc.DrawText(str(sv.CNO), w / CHANNEL * chn, self.h - bar)
def __init__(self):
self.w = WIDTH
self.h = WIDTH + BARH
wx.Frame.__init__(self, id=-1, parent=None, name=u'SVInfoFrame',
size=wx.Size(self.w, self.h), title=u'SV Info')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.sv = {}
self.interface = IvyMessagesInterface("svinfoframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class SVInfoFrame(wx.Frame):
def message_recv(self, ac_id, msg):
if msg.name == "SVINFO":
chn = int(msg['chn'])
self.sv[chn] = SvChannel(chn, msg)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize()[0]
self.h = event.GetSize()[1]
self.Refresh()
def OnPaint(self, e):
tdx = -5
tdy = -7
th = 15
w = self.w
h = self.w
if h < self.w + 50:
h = self.w + 50
bar = self.h - w - th - th
dc = wx.PaintDC(self)
brush = wx.Brush("white")
dc.SetBackground(brush)
dc.Clear()
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
dc.DrawCircle(w / 2, w / 2, w / 2 - 1)
dc.DrawCircle(w / 2, w / 2, w / 4 - 1)
dc.DrawCircle(w / 2, w / 2, 1)
font = wx.Font(11, wx.ROMAN, wx.BOLD, wx.NORMAL)
dc.SetFont(font)
dc.DrawText("N", w / 2 + tdx, 2)
dc.DrawText("S", w / 2 + tdx, w - 17)
dc.DrawText("E", w - 15, w / 2 + tdy)
dc.DrawText("W", 2, w / 2 + tdy)
# SV
for chn in self.sv:
sv = self.sv[chn]
c = QIColour(sv.QI)
used = sv.Flags & 1
s = 7 + used * 5
el = float(sv.Elev) / 90.0 * float(w) / 2.0
az = float(sv.Azim) * math.pi / 180.0
y = float(w) / 2.0 - math.cos(az) * el
x = float(w) / 2.0 + math.sin(az) * el
dc.SetBrush(wx.Brush(c, wx.SOLID))
dc.DrawCircle(int(x), int(y), s)
font = wx.Font(8, wx.ROMAN, wx.NORMAL, wx.NORMAL)
dc.SetFont(font)
dc.DrawText(str(sv.SVID), x + tdx, y + tdy)
bh = float(bar - th - th) * float(sv.CNO) / 55.0
dc.DrawRectangle(w / CHANNEL * chn + 5 * (1 - used),
self.h - th - bh,
w / CHANNEL - 2 - 10 * (1 - used), bh)
dc.DrawText(str(chn), w / CHANNEL * chn, self.h - th)
dc.DrawText(str(sv.CNO), w / CHANNEL * chn, self.h - bar)
def __init__(self):
self.w = WIDTH
self.h = WIDTH + BARH
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'SVInfoFrame',
size=wx.Size(self.w, self.h),
title=u'SV Info')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/svinfo/svinfo.ico",
wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.sv = {}
self.interface = IvyMessagesInterface("svinfoframe")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class AtcFrame(wx.Frame):
def message_recv(self, ac_id, msg):
self.callsign = "ID=" + str(ac_id)
if msg.name == "INS_REF":
self.qfe = round(float(msg['baro_qfe']) / 100.0, 1)
wx.CallAfter(self.update)
elif msg.name == "ROTORCRAFT_FP_MIN":
self.gspeed = round(float(msg['gspeed']) / 100.0 * 3.6 / 1.852, 1)
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084, 1)
wx.CallAfter(self.update)
elif msg.name == "ROTORCRAFT_FP":
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084, 1)
wx.CallAfter(self.update)
elif msg.name == "AIR_DATA":
self.airspeed = round(float(msg['airspeed']) * 3.6 / 1.852, 1)
self.qnh = round(float(msg['qnh']), 1)
self.amsl = round(float(msg['amsl_baro']) * 3.28084, 1)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w))
self.cfg.Write("height", str(self.h))
self.Refresh()
def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x))
self.cfg.Write("top", str(self.y))
def OnPaint(self, e):
w = self.w
h = self.h
if (float(w) / float(h)) > (WIDTH / HEIGHT):
w = int(h * WIDTH / HEIGHT)
else:
h = int(w * HEIGHT / WIDTH)
tdy = int(w * 75.0 / WIDTH)
tdx = int(w * 15.0 / WIDTH)
dc = wx.PaintDC(self)
fontscale = int(w * 40.0 / WIDTH)
if fontscale < 6:
fontscale = 6
# Background
dc.SetBrush(wx.Brush(wx.Colour(0, 0, 0), wx.TRANSPARENT))
#dc.DrawCircle(w/2,w/2,w/2-1)
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL,
wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("Callsign: " + str(self.callsign) + " ", tdx,
tdx + tdy * 0)
dc.DrawText("Airspeed: " + str(self.airspeed) + " kt", tdx,
tdx + tdy * 1)
dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt", tdx,
tdx + tdy * 2)
dc.DrawText("AMSL: " + str(self.amsl) + " ft (<2700ft)", tdx,
tdx + tdy * 3)
dc.DrawText("AGL: " + str(self.alt) + " ft (<1500ft)", tdx,
tdx + tdy * 4)
dc.DrawText(
"QNH: " + str(self.qnh * 100.0) + " QFE: " + str(self.qfe) + "",
tdx, tdx + tdy * 5)
def __init__(self):
self.w = WIDTH
self.h = HEIGHT
self.airspeed = 0
self.amsl = 0
self.qnh = 0
self.qfe = 0
self.alt = 0
#self.hmsl = 0;
self.gspeed = 0
self.callsign = ""
self.safe_to_approach = ""
self.cfg = wx.Config('atc_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'ATC Center',
size=wx.Size(self.w, self.h),
title=u'ATC Center')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico",
wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("ATC-Center")
self.interface.subscribe(self.message_recv)
if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x, self.y), wx.SIZE_USE_EXISTING)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
class CommandSender(IvyMessagesInterface):
def __init__(self, verbose=False, callback = None):
self.verbose = verbose
self.callback = callback
self._interface = IvyMessagesInterface("Mission Commander", start_ivy=False)
self._interface.subscribe(self.message_recv)
self._interface.start()
def message_recv(self, ac_id, msg):
if (self.verbose and self.callback != None):
self.callback(ac_id, msg)
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def add_mission_command_dict(self, ac_id, insert, msg_id, msgs):
msg = PprzMessage("datalink", msg_id)
msg['ac_id'] = ac_id
msg['insert'] = insert
msg['duration'] = msgs.get('duration')
msg['index'] = msgs.get('index')
if msg_id == 'MISSION_GOTO_WP_LLA':
msg['wp_lat'] = msgs.get('wp_lat')
msg['wp_lon'] = msgs.get('wp_lon')
msg['wp_alt'] = msgs.get('wp_alt')
elif msg_id == 'MISSION_CIRCLE_LLA':
msg['center_lat'] = msgs.get('center_lat')
msg['center_lon'] = msgs.get('center_lon')
msg['center_alt'] = msgs.get('center_alt')
msg['radius'] = msgs.get('radius')
elif msg_id == 'MISSION_SEGMENT_LLA':
msg['segment_lat_1'] = msgs.get('segment_lat_1')
msg['segment_lon_1'] = msgs.get('segment_lon_1')
msg['segment_lat_2'] = msgs.get('segment_lat_2')
msg['segment_lon_2'] = msgs.get('segment_lon_2')
elif msg_id == 'MISSION_PATH_LLA':
msg['point_lat_1'] = msgs.get('point_lat_1')
msg['point_lon_1'] = msgs.get('point_lon_1')
msg['point_lat_2'] = msgs.get('point_lat_2')
msg['point_lon_2'] = msgs.get('point_lon_2')
msg['point_lat_3'] = msgs.get('point_lat_3')
msg['point_lon_3'] = msgs.get('point_lon_3')
msg['point_lat_4'] = msgs.get('point_lat_4')
msg['point_lon_4'] = msgs.get('point_lon_4')
msg['point_lat_5'] = msgs.get('point_lat_5')
msg['point_lon_5'] = msgs.get('point_lon_5')
msg['path_alt'] = msgs.get('path_alt')
msg['nb'] = msgs.get('nb')
elif msg_id == 'MISSION_SURVEY_LLA':
msg['survey_lat_1'] = msgs.get('survey_lat_1')
msg['survey_lon_1'] = msgs.get('survey_lon_1')
msg['survey_lat_2'] = msgs.get('survey_lat_2')
msg['survey_lon_2'] = msgs.get('survey_lon_2')
msg['survey_alt'] = msgs.get('survey_alt')
# print("Sending message: %s" % msg)
self._interface.send(msg)
def add_shape(self, status, obstacle_id, obmsg):
msg = PprzMessage("ground", "SHAPE")
msg['id'] = int(obstacle_id)
msg['fillcolor'] = "red" if obstacle_id > 19 else "orange"
msg['linecolor'] = "red" if obstacle_id > 19 else "orange"
msg['status'] = 0 if status=="create" else 1
msg['shape'] = 0
msg['latarr'] = [int(obmsg.get("latitude")*10000000.),int(obmsg.get("latitude")*10000000.)]
msg['lonarr'] = [int(obmsg.get("longitude")*10000000.),int(obmsg.get("longitude")*10000000.)]
msg['radius'] = int(obmsg.get("sphere_radius") if "sphere_radius" in obmsg else obmsg.get("cylinder_radius"))
msg['text'] = "NULL"
msg['opacity'] = 2
self._interface.send(msg)
class MessagesFrame(wx.Frame):
def message_recv(self, ac_id, msg):
"""Handle incoming messages
Callback function for IvyMessagesInterface
:param ac_id: aircraft id
:type ac_id: int
:param msg: message
:type msg: PprzMessage
"""
# only show messages of the requested class
if msg.msg_class != self.msg_class:
return
if ac_id in self.aircrafts and msg.name in self.aircrafts[
ac_id].messages:
if time.time() - self.aircrafts[ac_id].messages[
msg.name].last_seen < 0.2:
return
wx.CallAfter(self.gui_update, ac_id, msg)
def find_page(self, book, name):
if book.GetPageCount() < 1:
return 0
start = 0
end = book.GetPageCount()
while start < end:
if book.GetPageText(start) >= name:
return start
start += 1
return start
def update_leds(self):
wx.CallAfter(self.update_leds_real)
def update_leds_real(self):
for ac_id in self.aircrafts:
aircraft = self.aircrafts[ac_id]
for msg_str in aircraft.messages:
message = aircraft.messages[msg_str]
if message.last_seen + 0.2 < time.time():
aircraft.messages_book.SetPageImage(message.index, 0)
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
def setup_image_list(self, notebook):
imageList = wx.ImageList(24, 24)
image = wx.Image(PPRZ_HOME + "/data/pictures/gray_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
image = wx.Image(PPRZ_HOME + "/data/pictures/green_led24.png")
bitmap = wx.BitmapFromImage(image)
imageList.Add(bitmap)
notebook.AssignImageList(imageList)
def add_new_aircraft(self, ac_id):
self.aircrafts[ac_id] = Aircraft(ac_id)
ac_panel = wx.Panel(self.notebook, -1)
self.notebook.AddPage(ac_panel, str(ac_id))
messages_book = wx.Notebook(ac_panel, style=wx.NB_LEFT)
self.setup_image_list(messages_book)
sizer = wx.BoxSizer(wx.VERTICAL)
sizer.Add(messages_book, 1, wx.EXPAND)
ac_panel.SetSizer(sizer)
sizer.Layout()
self.aircrafts[ac_id].messages_book = messages_book
def add_new_message(self, aircraft, msg_class, name):
messages_book = aircraft.messages_book
aircraft.messages[name] = Message(msg_class, name)
field_panel = wx.Panel(messages_book)
grid_sizer = wx.FlexGridSizer(len(aircraft.messages[name].fieldnames),
2)
index = self.find_page(messages_book, name)
messages_book.InsertPage(index, field_panel, name, imageId=1)
aircraft.messages[name].index = index
# update indexes of pages which are to be moved
for message_name in aircraft.messages:
aircraft.messages[message_name].index = self.find_page(
messages_book, message_name)
for field_name in aircraft.messages[name].fieldnames:
name_text = wx.StaticText(field_panel, -1, field_name)
size = name_text.GetSize()
size.x = LABEL_WIDTH
name_text.SetMinSize(size)
grid_sizer.Add(name_text, 1, wx.ALL, BORDER)
value_control = wx.StaticText(field_panel,
-1,
"42",
style=wx.EXPAND)
size = value_control.GetSize()
size.x = LABEL_WIDTH
value_control.SetMinSize(size)
grid_sizer.Add(value_control, 1, wx.ALL | wx.EXPAND, BORDER)
if wx.MAJOR_VERSION > 2:
if grid_sizer.IsColGrowable(1):
grid_sizer.AddGrowableCol(1)
else:
grid_sizer.AddGrowableCol(1)
aircraft.messages[name].field_controls.append(value_control)
field_panel.SetAutoLayout(True)
field_panel.SetSizer(grid_sizer)
field_panel.Layout()
def gui_update(self, ac_id, msg):
if ac_id not in self.aircrafts:
self.add_new_aircraft(ac_id)
aircraft = self.aircrafts[ac_id]
if msg.name not in aircraft.messages:
self.add_new_message(aircraft, msg.msg_class, msg.name)
aircraft.messages_book.SetPageImage(aircraft.messages[msg.name].index,
1)
self.aircrafts[ac_id].messages[msg.name].last_seen = time.time()
for index in range(0, len(msg.fieldvalues)):
aircraft.messages[msg.name].field_controls[index].SetLabel(
str(msg.get_field(index)))
def __init__(self, msg_class="telemetry"):
wx.Frame.__init__(self,
id=-1,
parent=None,
name=u'MessagesFrame',
size=wx.Size(WIDTH, HEIGHT),
style=wx.DEFAULT_FRAME_STYLE,
title=u'Messages')
self.Bind(wx.EVT_CLOSE, self.OnClose)
self.notebook = wx.Notebook(self)
self.aircrafts = {}
sizer = wx.BoxSizer(wx.HORIZONTAL)
sizer.Add(self.notebook, 1, wx.EXPAND)
self.SetSizer(sizer)
sizer.Layout()
self.timer = threading.Timer(0.1, self.update_leds)
self.timer.start()
self.msg_class = msg_class
self.interface = IvyMessagesInterface("Paparazzi Messages Viewer")
self.interface.subscribe(self.message_recv)
def OnClose(self, event):
self.timer.cancel()
self.interface.shutdown()
self.Destroy()
parser.add_argument("-c","--curl", action="store_true", help="dump actions as curl commands")
parser.add_argument("-s","--subscribe", action="store_true", help="subscribe to the ivy bus")
parser.add_argument("-v","--verbose", action="store_true", help="verbose mode")
try:
# --- Startup state initialization block
args = parser.parse_args()
static_init_configuration_data()
static_init_client_configuration_data(args.file)
if args.verbose:
print_aircraft_data()
if args.generate:
template_configuration()
sys.exit(0)
if args.subscribe:
ivy_interface.subscribe(callback_aircraft_messages)
ivy_interface.start()
# Handle misc. command line arguments
if args.verbose:
verbose=args.verbose
if args.curl:
curl=args.curl
print_curl_header(args.ip, args.port)
# --- Main loop
# Supply flask the appropriate ip address and port for the server
server_host = args.ip # Store for use in htlm template generation
server_port = args.port # Store for use in htlm template generation
app.run(host=args.ip,port=args.port,threaded=True)
class RadioBridge:
def __init__(self, link_uri, msg_class='telemetry', verbose=False):
""" Initialize and run with the specified link_uri """
self.verbose = verbose
# Ivy interface and stream parser
self._ivy = IvyMessagesInterface("cf2ivy")
self._transport = PprzTransport(msg_class)
# Create a Crazyradio
self._driver = RadioDriver()
self._driver.connect(link_uri, self._link_quality_cb,
self._link_error_cb)
if self.verbose:
print('Connecting to %s' % link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
# Bind to all messages from ac_id
def _forward_to_cf(ac_id, msg):
try:
data = self._transport.pack_pprz_msg(0,
msg) # sender_id 0 = GCS
for i in range(0, len(data), 30):
pk = CRTPPacket()
pk.port = CRTP_PORT_PPRZLINK
pk.data = data[i:(i + 30)]
self._driver.send_packet(pk)
if self.verbose:
print('Forward message', msg.name)
except:
if self.verbose:
print('Forward error for', ac_id)
messages_datalink = messages_xml_map.get_msgs("datalink")
for msg in messages_datalink:
self._ivy.subscribe(_forward_to_cf, PprzMessage("datalink", msg))
def shutdown(self):
if self.verbose:
print('closing cf2ivy interfaces')
self._ivy.shutdown()
self._driver.close()
def run(self):
pk = self._driver.receive_packet(
0.1) # wait for next message with timeout
if pk is not None:
self._got_packet(pk)
def _got_packet(self, pk):
if pk.port == CRTP_PORT_PPRZLINK:
for c in pk.data:
if self._transport.parse_byte(bytes([c])):
(sender_id, _, _, msg) = self._transport.unpack()
if self.verbose:
print("Got message {} from {}".format(
msg.name, sender_id))
# Forward message to Ivy bus
if self.is_connected:
self._ivy.send(msg, sender_id=sender_id)
def _link_quality_cb(self, quality):
pass
def _link_error_cb(self, msg):
if self.verbose:
print("Link error: {}".format(msg))
class AtcFrame(wx.Frame):
def message_recv(self, ac_id, msg):
self.callsign = "ID=" + str(ac_id)
if msg.name == "INS_REF":
self.qfe = round(float(msg['baro_qfe']) / 100.0,1)
wx.CallAfter(self.update)
elif msg.name =="ROTORCRAFT_FP_MIN":
self.gspeed = round(float(msg['gspeed']) / 100.0 * 3.6 / 1.852,1)
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
wx.CallAfter(self.update)
elif msg.name =="ROTORCRAFT_FP":
self.alt = round(float(msg['up']) * 0.0039063 * 3.28084 ,1)
wx.CallAfter(self.update)
elif msg.name =="AIR_DATA":
self.airspeed = round(float(msg['airspeed']) * 3.6 / 1.852,1)
self.qnh = round(float(msg['qnh']),1)
self.amsl = round(float(msg['amsl_baro']) * 3.28084,1)
wx.CallAfter(self.update)
def update(self):
self.Refresh()
def OnSize(self, event):
self.w = event.GetSize().x
self.h = event.GetSize().y
self.cfg.Write("width", str(self.w));
self.cfg.Write("height", str(self.h));
self.Refresh()
def OnMove(self, event):
self.x = event.GetPosition().x
self.y = event.GetPosition().y
self.cfg.Write("left", str(self.x));
self.cfg.Write("top", str(self.y));
def OnPaint(self, e):
w = self.w
h = self.h
if (float(w)/float(h)) > (WIDTH/HEIGHT):
w = int(h * WIDTH/HEIGHT)
else:
h = int(w * HEIGHT/WIDTH)
tdy = int(w * 75.0 / WIDTH)
tdx = int(w * 15.0 / WIDTH)
dc = wx.PaintDC(self)
fontscale = int(w * 40.0 / WIDTH)
if fontscale < 6:
fontscale = 6
# Background
dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
#dc.DrawCircle(w/2,w/2,w/2-1)
font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD)
dc.SetFont(font)
dc.DrawText("Callsign: " + str(self.callsign) + " ",tdx,tdx+tdy*0)
dc.DrawText("Airspeed: " + str(self.airspeed) + " kt",tdx,tdx+tdy*1)
dc.DrawText("Ground Speed: " + str(self.gspeed) + " kt",tdx,tdx+tdy*2)
dc.DrawText("AMSL: " + str(self.amsl) + " ft (<2700ft)",tdx,tdx+tdy*3)
dc.DrawText("AGL: " + str(self.alt) + " ft (<1500ft)",tdx,tdx+tdy*4)
dc.DrawText("QNH: " + str(self.qnh*100.0) + " QFE: " + str(self.qfe) + "",tdx,tdx+tdy*5)
def __init__(self):
self.w = WIDTH
self.h = HEIGHT
self.airspeed = 0;
self.amsl = 0;
self.qnh = 0;
self.qfe = 0;
self.alt = 0;
#self.hmsl = 0;
self.gspeed = 0;
self.callsign = ""
self.safe_to_approach = "";
self.cfg = wx.Config('atc_conf')
if self.cfg.Exists('width'):
self.w = int(self.cfg.Read('width'))
self.h = int(self.cfg.Read('height'))
wx.Frame.__init__(self, id=-1, parent=None, name=u'ATC Center',
size=wx.Size(self.w, self.h), title=u'ATC Center')
self.Bind(wx.EVT_PAINT, self.OnPaint)
self.Bind(wx.EVT_SIZE, self.OnSize)
self.Bind(wx.EVT_MOVE, self.OnMove)
self.Bind(wx.EVT_CLOSE, self.OnClose)
ico = wx.Icon(PPRZ_SRC + "/sw/ground_segment/python/atc/atc.ico", wx.BITMAP_TYPE_ICO)
self.SetIcon(ico)
self.interface = IvyMessagesInterface("ATC-Center")
self.interface.subscribe(self.message_recv)
if self.cfg.Exists('left'):
self.x = int(self.cfg.Read('left'))
self.y = int(self.cfg.Read('top'))
self.SetPosition(wx.Point(self.x,self.y), wx.SIZE_USE_EXISTING)
def OnClose(self, event):
self.interface.shutdown()
self.Destroy()
