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 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 example2():
# Usage: ./setting.py <path_var_AC>/settings.xml <ac_id>
ivy = IvyMessagesInterface("DemoSettings")
try:
setting_manager = PprzSettingsManager(sys.argv[1], sys.argv[2], ivy)
while True:
time.sleep(1)
setting_manager["altitude"] = setting_manager["altitude"].value + 2
except KeyboardInterrupt:
ivy.shutdown()
print("Stopping on request")
class PprzlinkDatastream(AbstractDatastream):
def initialize(self):
name = 'pprzlink'
self._interface = IvyMessagesInterface("pprzlink_morse")
@classproperty
def _type_url(cls):
return "http://docs.paparazziuav.org/latest/paparazzi_messages.html#" + cls._type_name
def finalize(self):
if self._interface is not None:
self._interface.shutdown()
self._interface = None
class WaypointMover(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface("WaypointMover")
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def move_waypoint(self, ac_id, wp_id, lat, lon, alt):
msg = PprzMessage("ground", "MOVE_WAYPOINT")
msg['ac_id'] = ac_id
msg['wp_id'] = wp_id
msg['lat'] = lat
msg['long'] = lon
msg['alt'] = alt
print("Sending message: %s" % msg)
self._interface.send(msg)
class WaypointMover(object):
def __init__(self, verbose=False):
self.verbose = verbose
self._interface = IvyMessagesInterface("WaypointMover")
def shutdown(self):
print("Shutting down ivy interface...")
self._interface.shutdown()
def __del__(self):
self.shutdown()
def move_waypoint(self, ac_id, wp_id, lat, lon, alt):
msg = PprzMessage("ground", "MOVE_WAYPOINT")
msg['ac_id'] = ac_id
msg['wp_id'] = wp_id
msg['lat'] = lat
msg['long'] = lon
msg['alt'] = alt
print("Sending message: %s" % msg)
self._interface.send(msg)
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):
wait_step = 0.1
timeout = 30 / wait_step # 30 seconds
new_answer = False
def aircrafts_cb(ac_id, msg):
global new_answer
self.ac_list = [int(a) for a in msg['ac_list'].split(',') if a]
print("aircrafts: {}".format(self.ac_list))
new_answer = True
self._interface.send_request('ground', "AIRCRAFTS", aircrafts_cb)
# hack: sleep briefly to wait for answer
while not new_answer and timeout > 0:
sleep(wait_step)
timeout -= 1
if not new_answer:
print(
"WARNING: Getting the list of aircraft timed out. The results might be outdated."
)
# Didn't raise an exception or return None in order to not break the API
return self.ac_list
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 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)
class RtpViewer:
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
# Start an 'infinite' loop
while True:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved or 'q' is pressed
if not ret or cv2.waitKey(1) & 0xFF == ord('q'):
break
# Run the computer vision function
self.cv()
def cv(self):
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'], (0, 255, 0), 2)
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
ivy.send(gr)
# start natnet interface
natnet = NatNetClient(
server=args.server,
rigidBodyListListener=receiveRigidBodyList,
dataPort=args.data_port,
commandPort=args.command_port,
verbose=args.verbose)
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
try:
# Start up the streaming client.
# This will run perpetually, and operate on a separate thread.
natnet.run()
while True:
sleep(1)
except (KeyboardInterrupt, SystemExit):
print("Shutting down ivy and natnet interfaces...")
natnet.stop()
ivy.shutdown()
except OSError:
print("Natnet connection error")
natnet.stop()
ivy.stop()
exit(-1)
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 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()
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)
# --- Shutdown state block
ivy_interface.shutdown()
except Exception as e:
print(e)
sys.exit(0)
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 RtpViewer:
running = False
scale = 1
rotate = 0
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
self.running = True
# Start an 'infinite' loop
while self.running:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved
if not ret:
break
# Run the computer vision function
self.cv()
# Process key input
self.on_key(cv2.waitKey(1) & 0xFF)
def cv(self):
# Rotate the image by increments of 90
if self.rotate % 2:
self.frame = cv2.transpose(self.frame)
if self.rotate > 0:
self.frame = cv2.flip(self.frame, [1, -1, 0][self.rotate - 1])
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'],
(0, 255, 0), 2)
if self.scale != 1:
h, w = self.frame.shape[:2]
self.frame = cv2.resize(self.frame,
(int(self.scale * w), int(self.scale * h)))
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_key(self, key):
if key == ord('q'):
self.running = False
if key == ord('r'):
self.rotate = (self.rotate + 1) % 4
self.mouse['start'] = None
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN and self.rotate == 0:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
class RtpViewer:
running = False
scale = 1
rotate = 0
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
self.running = True
# Start an 'infinite' loop
while self.running:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved
if not ret:
break
# Run the computer vision function
self.cv()
# Process key input
self.on_key(cv2.waitKey(1) & 0xFF)
def cv(self):
# Rotate the image by increments of 90
if self.rotate % 2:
self.frame = cv2.transpose(self.frame)
if self.rotate > 0:
self.frame = cv2.flip(self.frame, [1, -1, 0][self.rotate - 1])
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'], (0, 255, 0), 2)
if self.scale != 1:
h, w = self.frame.shape[:2]
self.frame = cv2.resize(self.frame, (int(self.scale * w), int(self.scale * h)))
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_key(self, key):
if key == ord('q'):
self.running = False
if key == ord('r'):
self.rotate = (self.rotate + 1) % 4
self.mouse['start'] = None
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN and self.rotate == 0:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
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 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 initTable:
def __init__(self, config, verbose=False):
self.verbose = verbose
self.config = config
self.ids = np.array(self.config['ids'])
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.list_ids = np.ndarray.tolist(self.ids)
# Start IVY interface
self._interface = IvyMessagesInterface("Init DCF tables")
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def init_dcftables(self):
time.sleep(2)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
# nei_id = 0, special msg to clean the table onboard
msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
msg_clean_a['nei_id'] = 0
msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
msg_clean_b['nei_id'] = 0
self._interface.send(msg_clean_a)
self._interface.send(msg_clean_b)
if self.verbose:
print(msg_clean_a)
print(msg_clean_b)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
msga = PprzMessage("datalink", "DCF_REG_TABLE")
msga['ac_id'] = int(self.list_ids[i[0]])
msga['nei_id'] = int(self.list_ids[i[1]])
if len(self.list_ids) == 2:
msga['desired_sigma'] = (column[index])[0] * int(self.Zdesired)
else:
msga['desired_sigma'] = (column[index])[0] * int(
self.Zdesired[count])
self._interface.send(msga)
msgb = PprzMessage("datalink", "DCF_REG_TABLE")
msgb['ac_id'] = int(self.list_ids[i[1]])
msgb['nei_id'] = int(self.list_ids[i[0]])
if len(self.list_ids) == 2:
msgb['desired_sigma'] = (column[index])[1] * int(self.Zdesired)
else:
msgb['desired_sigma'] = (column[index])[1] * int(
self.Zdesired[count])
self._interface.send(msgb)
if self.verbose:
print(msga)
print(msgb)
time.sleep(2)
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 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):
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 initTable:
def __init__(self, config, verbose=False):
self.verbose = verbose
self.config = config
self.ids = np.array(self.config['ids'])
self.B = np.array(self.config['topology'])
self.Zdesired = np.array(self.config['desired_intervehicle_angles'])
self.list_ids = np.ndarray.tolist(self.ids)
# Start IVY interface
self._interface = IvyMessagesInterface("Init DCF tables")
def __del__(self):
self.stop()
def stop(self):
# Stop IVY interface
if self._interface is not None:
self._interface.shutdown()
def init_dcftables(self):
time.sleep(2)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
# nei_id = 0, special msg to clean the table onboard
msg_clean_a = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_a['ac_id'] = int(self.list_ids[i[0]])
msg_clean_a['nei_id'] = 0
msg_clean_b = PprzMessage("datalink", "DCF_REG_TABLE")
msg_clean_b['ac_id'] = int(self.list_ids[i[1]])
msg_clean_b['nei_id'] = 0
self._interface.send(msg_clean_a)
self._interface.send(msg_clean_b)
if self.verbose:
print(msg_clean_a)
print(msg_clean_b)
for count, column in enumerate(self.B.T):
index = np.nonzero(column)
i = index[0]
msga = PprzMessage("datalink", "DCF_REG_TABLE")
msga['ac_id'] = int(self.list_ids[i[0]])
msga['nei_id'] = int(self.list_ids[i[1]])
if len(self.list_ids) == 2:
msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired)
else:
msga['desired_sigma'] = (column[index])[0]*int(self.Zdesired[count])
self._interface.send(msga)
msgb = PprzMessage("datalink", "DCF_REG_TABLE")
msgb['ac_id'] = int(self.list_ids[i[1]])
msgb['nei_id'] = int(self.list_ids[i[0]])
if len(self.list_ids) == 2:
msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired)
else:
msgb['desired_sigma'] = (column[index])[1]*int(self.Zdesired[count])
self._interface.send(msgb)
if self.verbose:
print(msga)
print(msgb)
time.sleep(2)
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 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 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 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 Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.auto2_index = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.auto2_index = settings.name_lookup['auto2'].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("guided mode example")
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 set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg['ac_id'] = self.ac_id
msg['index'] = self.auto2_index
msg['value'] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x00
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0D
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg['ac_id'] = self.ac_id
msg['flags'] = 0x0E
msg['x'] = forward
msg['y'] = right
msg['z'] = down
msg['yaw'] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_ned_vel(self, north=0.0, east=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'] = 0x60
msg['x'] = north
msg['y'] = east
msg['z'] = down
msg['yaw'] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(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'] = 0x62
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)
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)
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 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 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 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 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 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 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 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 RtpViewer:
frame = None
mouse = dict()
def __init__(self, src):
# Create the video capture device
self.cap = cv2.VideoCapture(src)
# Start the ivy interface
self.ivy = IvyMessagesInterface("RTPviewer", start_ivy=False)
self.ivy.start()
# Create a named window and add a mouse callback
cv2.namedWindow('rtp')
cv2.setMouseCallback('rtp', self.on_mouse)
def run(self):
# Start an 'infinite' loop
while True:
# Read a frame from the video capture
ret, self.frame = self.cap.read()
# Quit if frame could not be retrieved or 'q' is pressed
if not ret or cv2.waitKey(1) & 0xFF == ord('q'):
break
# Run the computer vision function
self.cv()
def cv(self):
# If a selection is happening
if self.mouse.get('start'):
# Draw a rectangle indicating the region of interest
cv2.rectangle(self.frame, self.mouse['start'], self.mouse['now'],
(0, 255, 0), 2)
# Show the image in a window
cv2.imshow('rtp', self.frame)
def on_mouse(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouse['start'] = (x, y)
if event == cv2.EVENT_RBUTTONDOWN:
self.mouse['start'] = None
if event == cv2.EVENT_MOUSEMOVE:
self.mouse['now'] = (x, y)
if event == cv2.EVENT_LBUTTONUP:
# If mouse start is defined, a region has been selected
if not self.mouse.get('start'):
return
# Obtain mouse start coordinates
sx, sy = self.mouse['start']
# Create a new message
msg = PprzMessage("datalink", "VIDEO_ROI")
msg['ac_id'] = None
msg['startx'] = sx
msg['starty'] = sy
msg['width'] = abs(x - sx)
msg['height'] = abs(y - sy)
msg['downsized_width'] = self.frame.shape[1]
# Send message via the ivy interface
self.ivy.send_raw_datalink(msg)
# Reset mouse start
self.mouse['start'] = None
def cleanup(self):
# Shutdown ivy interface
self.ivy.shutdown()
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 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 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()
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)
# --- Shutdown state block
ivy_interface.shutdown()
except Exception as e:
print(e)
sys.exit(0)
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 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))
# convert quaternion to psi euler angle
dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
ivy.send(msg)
# start natnet interface
natnet = NatNetClient(server=args.server,
rigidBodyListListener=receiveRigidBodyList,
dataPort=args.data_port,
commandPort=args.command_port,
verbose=args.verbose)
print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
try:
# Start up the streaming client.
# This will run perpetually, and operate on a separate thread.
natnet.run()
while True:
sleep(1)
except (KeyboardInterrupt, SystemExit):
print("Shutting down ivy and natnet interfaces...")
natnet.stop()
ivy.shutdown()
except OSError:
print("Natnet connection error")
natnet.stop()
ivy.stop()
exit(-1)
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 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 Guidance(object):
def __init__(self, ac_id):
self.ac_id = ac_id
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("sim_rc_4ch")
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 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 rc_4_channel_output(self,
mode=2,
throttle=0.0,
roll=0.0,
pitch=0.0,
yaw=0.0):
"""
move at specified velocity in meters/sec with absolute heading (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "RC_4CH")
msg['ac_id'] = self.ac_id
msg['mode'] = mode
msg['throttle'] = throttle
msg['roll'] = roll
msg['pitch'] = pitch
msg['yaw'] = yaw
print("move at vel body: %s" % msg)
self._interface.send_raw_datalink(msg)
class Guidance(object):
def __init__(self, ac_id, verbose=False):
self.ac_id = ac_id
self.verbose = verbose
self._interface = None
self.auto2_index = None
try:
settings = PaparazziACSettings(self.ac_id)
except Exception as e:
print(e)
return
try:
self.auto2_index = settings.name_lookup["auto2"].index
except Exception as e:
print(e)
print("auto2 setting not found, mode change not possible.")
self._interface = IvyMessagesInterface("guided mode example")
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 set_guided_mode(self):
"""
change auto2 mode to GUIDED.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 19 # AP_MODE_GUIDED
print("Setting mode to GUIDED: %s" % msg)
self._interface.send(msg)
def set_nav_mode(self):
"""
change auto2 mode to NAV.
"""
if self.auto2_index is not None:
msg = PprzMessage("ground", "DL_SETTING")
msg["ac_id"] = self.ac_id
msg["index"] = self.auto2_index
msg["value"] = 13 # AP_MODE_NAV
print("Setting mode to NAV: %s" % msg)
self._interface.send(msg)
def goto_ned(self, north, east, down, heading=0.0):
"""
goto a local NorthEastDown position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x00
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = heading
print("goto NED: %s" % msg)
# embed the message in RAW_DATALINK so that the server can log it
self._interface.send_raw_datalink(msg)
def goto_ned_relative(self, north, east, down, yaw=0.0):
"""
goto a local NorthEastDown position relative to current position in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x01
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = yaw
print("goto NED relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def goto_body_relative(self, forward, right, down, yaw=0.0):
"""
goto to a position relative to current position and heading in meters (if already in GUIDED mode)
"""
msg = PprzMessage("datalink", "GUIDED_SETPOINT_NED")
msg["ac_id"] = self.ac_id
msg["flags"] = 0x03
msg["x"] = forward
msg["y"] = right
msg["z"] = down
msg["yaw"] = yaw
print("goto body relative: %s" % msg)
self._interface.send_raw_datalink(msg)
def move_at_vel(self, north=0.0, east=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"] = 0x70
msg["x"] = north
msg["y"] = east
msg["z"] = down
msg["yaw"] = yaw
print("move at vel NED: %s" % msg)
self._interface.send_raw_datalink(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()
