From cf9499012ed2754b660adbc6248be828b51185e3 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Fri, 5 Oct 2018 22:02:53 +0200 Subject: [PATCH] Update multiple python tools (#2333) * fix missing path, remove unused code, use more common fonts, make text rescale * Save window position on close, better resize and font change, wind plotter, updated battery model * Fixed merge problem, cleaned up and added to controlpanel * Cleanup wind --- conf/control_panel_example.xml | 2 + conf/userconf/tudelft/control_panel.xml | 2 + sw/ground_segment/python/atc/atc_frame.py | 84 ++++-- .../python/energy_mon/battery_model.py | 77 +++++- .../python/energy_mon/energy_mon_viewer.py | 252 +++++++++++++++--- .../python/svinfo/svinfoviewer.py | 27 +- sw/ground_segment/python/wind/wind.png | Bin 0 -> 3641 bytes sw/ground_segment/python/wind/wind.py | 37 +++ sw/ground_segment/python/wind/wind_frame.py | 210 +++++++++++++++ 9 files changed, 620 insertions(+), 71 deletions(-) create mode 100755 sw/ground_segment/python/wind/wind.png create mode 100755 sw/ground_segment/python/wind/wind.py create mode 100644 sw/ground_segment/python/wind/wind_frame.py diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml index 9a63a31a41c..8fb0e68c5af 100644 --- a/conf/control_panel_example.xml +++ b/conf/control_panel_example.xml @@ -69,6 +69,8 @@ + + diff --git a/conf/userconf/tudelft/control_panel.xml b/conf/userconf/tudelft/control_panel.xml index 6526b90723a..e429c3ae794 100644 --- a/conf/userconf/tudelft/control_panel.xml +++ b/conf/userconf/tudelft/control_panel.xml @@ -59,6 +59,8 @@ + + diff --git a/sw/ground_segment/python/atc/atc_frame.py b/sw/ground_segment/python/atc/atc_frame.py index e61bec2ed0b..30aad7e1f65 100644 --- a/sw/ground_segment/python/atc/atc_frame.py +++ b/sw/ground_segment/python/atc/atc_frame.py @@ -23,24 +23,28 @@ import time import threading import math -import pynotify import array from cStringIO import StringIO - PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), - '../../../..'))) + '../../../..'))) + +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../..'))) sys.path.append(PPRZ_HOME + "/var/lib/python") from pprzlink.ivy import IvyMessagesInterface -WIDTH = 300 +HEIGHT = 580.0 +WIDTH = 700.0 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) @@ -60,45 +64,58 @@ def message_recv(self, ac_id, msg): 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): - tdx = 10 - tdy = 80 w = self.w - h = 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) - #brush = wx.Brush("white") - #dc.SetBackground(brush) - #dc.Clear() + + 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(40, wx.ROMAN, wx.BOLD, wx.NORMAL) + font = wx.Font(fontscale, wx.FONTFAMILY_ROMAN, wx.FONTSTYLE_NORMAL, wx.FONTWEIGHT_BOLD) 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("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("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) + 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 = 900 - self.h = 700 + self.w = WIDTH + self.h = HEIGHT self.airspeed = 0; @@ -110,12 +127,22 @@ def __init__(self): #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) @@ -124,6 +151,11 @@ def __init__(self): 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() diff --git a/sw/ground_segment/python/energy_mon/battery_model.py b/sw/ground_segment/python/energy_mon/battery_model.py index edbd1e688bb..364b3a05992 100755 --- a/sw/ground_segment/python/energy_mon/battery_model.py +++ b/sw/ground_segment/python/energy_mon/battery_model.py @@ -20,18 +20,50 @@ import os import numpy as np +from scipy import interpolate PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../..'))) batmodel = np.loadtxt(PPRZ_SRC + '/sw/ground_segment/python/energy_mon/batterymodel.csv', delimiter=',') +batmodel_reversed = batmodel[::-1, :] +f_batmodel = interpolate.RectBivariateSpline(batmodel_reversed[:, 0], range(2, 11, 2), batmodel_reversed[:, 1:]) +# batmodel_wh = scipy.integrate.cumtrapz(batmodel[:, 1:], ) +# batmodel_power = batmodel.copy() +# batmodel_power[1:] *= np.array([2, 4, 6, 8, 10]) + +# Indices: +# 0: V +# 1: mAh 2A +# 2: mAh 4A +# 2: mAh 6A +# 2: mAh 8A +# 2: mAh 10A capacity = 3300 #mAh cells_in_series = 6 #cells cells_in_parallel = 6 #cells +cells_in_battery = cells_in_parallel * cells_in_series + +def interpolate_monotonic_x(x, y, x_test): + """Interpolate a function defined on a monotonically increasing grid + :param x -> x-data, must be monotonically increasing/decreasing (this is not checked + :param y -> corresponding y-data + :param x_test -> the x value for which you want the interpolated y + """ + n = len(x) + dx = x[1] - x[0] + x0 = [0] + + i_float = (x_test - x0) / dx + + assert i_float >= 0 and i_float <= len(y) + + i_lower = int(i_float) + return y[i_lower] + (y[i_lower + 1] - y[i_lower]) / (i_float - i_lower) def index_from_volt(volt): @@ -44,24 +76,49 @@ def index_from_volt(volt): # index in a list from 420:-1:250 index = 420 - v - return index + return int(index) def mah_from_volt_and_current(volt, current): - global batmodel - item = index_from_volt(volt) - mah = batmodel[item,1:] + global f_batmodel + return f_batmodel(volt, current) # (current - a0) / da * dm + m0 - # interpolate between point 1 and 2 - m0 = mah[1] - a0 = 4.0 - dm = mah[1]-mah[0] - da = 2.0 +def volt_amp_from_mAh_power(mAh, power): + Vs = np.zeros(5) + powers = np.zeros(5) + for i in range(5): + Vs[i] = np.interp(mAh, batmodel[:, i+1], batmodel[:, 0]) + I = i * 2 + 2 + powers[i] = Vs[i] * I - return (current - a0) / da * dm + m0 + volt = np.interp(power, powers, Vs) + amp = np.interp(power, powers, range(2, 11, 2)) + return volt, amp +def time_mAh_from_volt_to_volt_power(v0, v1, power): + mAh_accumulated = 0 + t_accumulated = 0 + mAh_prev = None + + for v in np.arange(v0, v1, -0.01): + I = power / v # current in A + mAh = mah_from_volt_and_current(v, I) + dmAh = mAh - mAh_prev if not mAh_prev is None else 0 + mAh_prev = mAh + dt = (dmAh / 1000) / I * 3600 + + mAh_accumulated += dmAh + t_accumulated += dt + + return t_accumulated, mAh_accumulated + #print(mah_from_volt_and_current(3.6, 2.5)) #for i in batmodel[:,[0,3]]: # print(i) + +if __name__ == '__main__': + v, i = volt_amp_from_mAh_power(2500, 550/cells_in_battery) + print(v, i, v*i*cells_in_battery) + print(time_mAh_from_volt_to_volt_power(3.3, 3.0, 550/cells_in_battery)) diff --git a/sw/ground_segment/python/energy_mon/energy_mon_viewer.py b/sw/ground_segment/python/energy_mon/energy_mon_viewer.py index bc9e8a6efe2..73d89f9416b 100644 --- a/sw/ground_segment/python/energy_mon/energy_mon_viewer.py +++ b/sw/ground_segment/python/energy_mon/energy_mon_viewer.py @@ -22,6 +22,7 @@ import sys import os import math +import datetime import battery_model as bat @@ -39,18 +40,13 @@ BARH = 140 -def QIColour(qi): - return { - 0: wx.Colour(64, 64, 64), # This channel is idle - 1: wx.Colour(128, 128, 128), # Searching - 2: wx.Colour(0, 128, 128), # Signal aquired - 3: wx.Colour(255, 0, 0), # Signal detected but unusable - 4: wx.Colour(0, 0, 255), # Code Lock on Signal - 5: wx.Colour(0, 255, 0), # Code and Carrier locked - 6: wx.Colour(0, 255, 0), # Code and Carrier locked - 7: wx.Colour(0, 255, 0), # Code and Carrier locked - }[qi] +def get_text_from_seconds(secs): + m, s = divmod(int(secs), int(60)) + return "{:02d}:{:02d}".format(m, s) +class AirDataMessage(object): + def __init__(self, msg): + self.airspeed = float(msg['airspeed']) class EnergyMessage(object): def __init__(self, msg): @@ -66,11 +62,11 @@ def __init__(self, msg): class BatteryCell(object): def __init__(self): - self.voltage = 0; - self.current = 0; - self.energy = 0; - self.model = 0; - self.temperature = 0; + self.voltage = 0 + self.current = 0 + self.energy = 0 + self.model = 0 + self.temperature = 0 def fill_from_energy_msg(self, energy): self.voltage = energy.volt / bat.cells_in_series self.current = energy.current / bat.cells_in_parallel @@ -89,20 +85,29 @@ def get_energy(self): return "Cell mAh = "+str(round(self.energy/1000.0 ,2)) + " Ah" def get_temp(self): return "Cell Temp = "+str(round(self.temperature ,2)) - + def get_power_text(self): + return "Battery Power: {:.0f}W".format(self.get_power() * bat.cells_in_battery) def get_volt_perc(self): - return (self.voltage - 2.5) / (4.2 - 2.5); + return self.get_volt_percent(self.voltage) + def get_volt_percent(self,volt): + return (volt - 2.5) / (4.3 - 2.5) + def get_power(self): + return self.voltage * self.current + def get_power_per_cell(self): + return self.get_power() / bat.cells_in_parallel / bat.cells_in_series + def get_temp_perc(self): + return (self.temperature / 60) def get_current_perc(self): return (self.current / 10) def get_energy_perc(self): return (self.energy / bat.capacity) def get_model_perc(self): return (self.model / bat.capacity) - def get_temp_perc(self): - return (self.temperature / 60); + def get_power_perc(self): + return (self.get_power() - 200) / (800 - 200) def get_volt_color(self): - if self.voltage < 3.4: + if self.voltage < 3.2: return 0.1 elif self.voltage < 3.6: return 0.5 @@ -125,9 +130,119 @@ def get_energy_color(self): def get_temp_color(self): if (self.temperature > 20) & (self.temperature < 40): return 1 - elif (self.temperature > 10) & (self.temperature < 50): + elif (self.temperature > 10) & (self.temperature < 55): + return 0.5 + return 0.1 + + def get_power_color(self): + return 0.5 + +class EnergyPrediction(object): + coeffs_power_from_airspeed = [2.9229, -95.559, 1029.8] # Coefficients from matlab + energy_land = 3 # Wh required to land + energy_takeoff = 3 # Wh required for take-off + charge_land = 0.3 * 1000 # mAh for battery + power_hover = 700 # W required for hover per battery + power_hover_cell = power_hover / bat.cells_in_battery + min_allowable_voltage = 3.0 + expected_landing_time = 30 + + def __init__(self, battery_cell): + self.battery_cell = battery_cell + self.airspeed = 0 + + def fill_from_air_data_msg(self, air_data): + self.airspeed = air_data.airspeed + + def get_expected_power_from_airspeed(self, airspeed): + return sum(self.coeffs_power_from_airspeed[i] * airspeed ** (2 - i) for i in range(3)) + + def get_expected_power(self): + """Calculate expected power based on airspeed; if airspeed < 15, this model is invalid and hover power is assumed + Power for whole battery""" + if self.airspeed > 15: + return self.get_expected_power_from_airspeed(self.airspeed) + return self.power_hover + + def get_power_fraction(self): + return self.battery_cell.get_power() * bat.cells_in_battery / self.get_expected_power() + + def get_power_fraction_text(self): + return "Fraction: {:.2f}".format(self.get_power_fraction()) + + def get_power_fraction_color(self): + if self.get_power_fraction() > 1.2: return 0.1 - return 0 + if self.get_power_fraction() > 1.0: + return 0.5 + return 1 + + def get_time_to_empty_battery_from_power(self, power): + volt, amp = bat.volt_amp_from_mAh_power(self.battery_cell.model, power) + if volt >= self.min_allowable_voltage: + time_to_empty_battery, _ = bat.time_mAh_from_volt_to_volt_power(volt, self.min_allowable_voltage, power) + else: + time_to_empty_battery = 0 + return time_to_empty_battery + + def get_hover_seconds_color(self): + if self.get_hover_seconds_left() > 90: + return 1 + if self.get_hover_seconds_left() > 30: + return 0.5 + return 0.1 + + def get_hover_seconds_fraction(self): + fraction = self.get_hover_seconds_left() / 120 + return min(1, fraction) + + def get_hover_seconds_left(self): + return self.get_time_to_empty_battery_from_power(self.power_hover_cell) + + def get_hover_seconds_left_text(self): + return "{} hover left".format(get_text_from_seconds(self.get_hover_seconds_left())) + + def get_fw_seconds_color(self): + if self.get_fw_seconds_left() > 120: + return 1 + if self.get_fw_seconds_left() > 60: + return 0.5 + return 0.1 + + def get_fw_seconds_fraction(self): + fraction = self.get_fw_seconds_left() / 120 + return min(1, fraction) + + def get_fw_seconds_left(self): + return self.get_time_to_empty_battery_from_power(self.get_expected_power() / bat.cells_in_battery) + + def get_fw_seconds_left_text(self): + return "{} fw left".format(get_text_from_seconds(self.get_fw_seconds_left())) + + def get_fw_seconds_left_20mps(self): + return self.get_time_to_empty_battery_from_power(self.get_expected_power_from_airspeed(20) / bat.cells_in_battery) + + def get_fw_seconds_left_20mps_text(self): + return "{} left@20".format(get_text_from_seconds(self.get_fw_seconds_left_20mps())) + + def get_fw_seconds_left_20mps_fraction(self): + fraction = self.get_fw_seconds_left_20mps() / 120 + return min(1, fraction) + + def get_fw_seconds_left_20mps_color(self): + if self.get_fw_seconds_left_20mps() > 120: + return 1 + if self.get_fw_seconds_left_20mps() > 60: + return 0.5 + return 0.1 + + + def get_max_hover_charge(self): + vmin = self.min_allowable_voltage + Areq = self.power_hover / bat.cells_in_series / bat.cells_in_parallel / vmin + mAh_vmin = bat.mah_from_volt_and_current(vmin, Areq) + return mAh_vmin + class EnergyMonFrame(wx.Frame): def message_recv(self, ac_id, msg): @@ -141,14 +256,28 @@ def message_recv(self, ac_id, 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()[0] - self.h = event.GetSize()[1] + 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 @@ -163,16 +292,18 @@ def StatusBox(self, dc, nr, txt, percent, color): 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.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)) + 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)) @@ -189,16 +320,29 @@ def DischargePlot(self, dc): 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,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(255,180,0),4)) + 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]] @@ -259,6 +403,11 @@ def OnPaint(self, e): 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) @@ -267,10 +416,22 @@ 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) @@ -278,7 +439,8 @@ def __init__(self): self.bat = {} self.temp = {} - self.cell = BatteryCell(); + self.cell = BatteryCell() + self.energy_prediction = EnergyPrediction(self.cell) self.interface = IvyMessagesInterface("energymonframe") self.interface.subscribe(self.message_recv) @@ -286,3 +448,29 @@ def __init__(self): def OnClose(self, event): self.interface.shutdown() self.Destroy() + +if __name__ == '__main__': + energy_message = EnergyMessage({"bat": 22, "amp": 18, "power": 22 * 18, "energy": 10000}) + air_data_message = AirDataMessage({"airspeed": 21}) + cell = BatteryCell() + cell.fill_from_energy_msg(energy_message) + + energy_prediction = EnergyPrediction(cell) + energy_prediction.fill_from_air_data_msg(air_data_message) + + print(energy_prediction.get_expected_power()) + print(energy_prediction.get_hover_seconds_left_text()) + print(energy_prediction.get_fw_seconds_left_text()) + print(energy_prediction.get_fw_seconds_left_20mps_text()) + import matplotlib.pyplot as plt + import numpy as np + energies = np.arange(0, 3200*6, 10) + seconds_left = np.zeros(energies.shape) + + # for i, energy in enumerate(energies): + # cell.fill_from_energy_msg(EnergyMessage({"bat": 22, "amp": 18, "power": 22 * 18, "energy": energy})) + # seconds_left[i] = energy_prediction.get_hover_seconds_left() + # + # + # plt.plot(energies, seconds_left) + # plt.show() diff --git a/sw/ground_segment/python/svinfo/svinfoviewer.py b/sw/ground_segment/python/svinfo/svinfoviewer.py index 8aa435acee4..558dc763b60 100644 --- a/sw/ground_segment/python/svinfo/svinfoviewer.py +++ b/sw/ground_segment/python/svinfo/svinfoviewer.py @@ -25,7 +25,6 @@ PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../..'))) - PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), '../../../..'))) @@ -73,10 +72,19 @@ def update(self): self.Refresh() def OnSize(self, event): - self.w = event.GetSize()[0] - self.h = event.GetSize()[1] + 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): tdx = -5 tdy = -7 @@ -137,10 +145,23 @@ def __init__(self): self.w = WIDTH self.h = WIDTH + BARH + self.cfg = wx.Config('svinfo_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'SVInfoFrame', size=wx.Size(self.w, self.h), title=u'SV Info') + + 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/svinfo/svinfo.ico", wx.BITMAP_TYPE_ICO) diff --git a/sw/ground_segment/python/wind/wind.png b/sw/ground_segment/python/wind/wind.png new file mode 100755 index 0000000000000000000000000000000000000000..d1b53be0fb2a6175030041437cea6182f00893e3 GIT binary patch literal 3641 zcmeHKX;f3!7Cs?BC}J2)G$I7i1rjM3Y8Xsqa0m#9AW1+(1RRP60jY_Cg(S363o5h% zrj0|G6T)aMiZUePfQ-S?=(erV0KlMI7_cyhhCN>kzFM%3MTcw#O-~#qAj33eQ^Y0!t`;xS zC7VI^XGgci9RmP)XW@eNWC#vGk)DKZ-9*XSJ30JKU&k?hPbNPKS>q5U{kb5x8g73+ z>~yB4YWdf~+Gtm<)HWh?=PDw1f>2$Bq+|2LwiM9MuR`sp?JS07TUE9+l=GuJsc zGBDLTz-)S_&v~>lg;}Pbnw)QM9>`5-pPOoWy84~qt>8rcnf~eiVWl~v;v|1CqOk_d z-o2&Vre2}uq3P#{+r3PKdL{*)WNBVNom1T;ij5qM*GG^Lb7AdqlNEseph<~DJN7f$ zscFrCF0NIcL_0oucD(iE&n8iA#RA33E3G9c_P5$i?d=pg=pCG-xtoFo|rWfTPq5WG%=ei zhp5A~K0ATspL~O%u@Zhmd5*SWjZkr%YLF!}H(0p!1*|0=LJWm_q_EJ#=`C;9g8b|& zg~%7TKad}0-z-e`eQ^!N?*6ONHNhljyT{uzYi}jUL?{jj%JYd~QKfnD^3*IIZtGsb z6jeGYOsBka2WG@b5@IUpgmAy?xdbtDT{3Mpmifa1!0M&R)YcDr;~I7*a;G$Jh1^+F zBAmJS9YWxfR$UZp7&a4O$M>kQYRlG=_wSZyI!(t|*h((bIZD>t52wcGeDyvYo$K+W zI7Anh(6D1?b8LJ4V^qdh-~NV-mcrJj_Qs}VcYJIt>?#V?4qWng;Kla11-4E+bQHPR z|Jc2JEzz%bk|4VfhcskrzwTkZ*-#j6F*|ZnX@YjNV$7V$A3O=tu?S;ZWw?cC-j-vd zFbl-uC$jaSxw~5#%0J%R7VVL_oa(L2`D`2!;KXk`6||-^M%$^c%=nP0)f%=?3s@8OV<}9Q4~*^dY-fb|#JP$$(3ooa^D7`;#YJA>a$XD+{!i8q*C~1S6ry z&=Cze9lNH09eU4Y-GpI!5Yqj!E~SU^nlPMW^PT9yT^CbY{A)navquSVn~9GucU_>B zG(4y@FQ2;2I`grokLA0E{Lf4ov=rt`UyhANM6wDR{%$lQZHi21xhBtwVjmv^_X|9o znH6E8Q>yT;?4>4+){u*FK#~zTwKUs2>U87{ry7XGzXu)IhmlOy^*sMgjDuj6%;j#8 zB;z-Ui)mrsbMWYERr~-V$-&dPB`VEI+<*|N8DJH}p7#uv?TTbRBmi4aXZ*4m2X-xz z>7EI$H0Y$XHG*Yzq?|2ta=Y$~z3;*;SUcoOOOIa$03llye;9tW)ZSpBcB)V7uT+oQ zy`nJM2FcpxD?Odx9Kgb(=Eg+99w$`sq42R{@|meo{q{1=CR}dYDL=B-?p2VyA|W<8 zIKo;?Ko~F3&Yw3=dKxYm%DM46j-j{fUI$|}zp~pG7J(FZKzI%U9r>**dYDy~cy-`l z?#)wvoBg*mR^Wrl@UalaTZSqg2OoQONaX2!%P1_NjMCM1-A+aGOVot?pe69J)RAy1 zeO_=2Au9UYh`ioS1#Fp3dpyfGziabTtVvH}c2`HQa}KD$R~NuEXA9H6yku=?>?dH@I|Dod6G(}7;;ErwZKv8c`v)aM$y7M> z@bQ+og&@J5+evfNI`+>_=yeGfIW~qPi>?qIp56r=6w-}Gu6oSZJgI+xLN`(k7_}#N zz~vOr=K%Pg-~|k<7dNQ%ZxT6APO4VGTS|F@+*xrwi~E7JgkxhBR9*nXi>&&?CnN1Y z^50iAbrD`ryz^kYBm=)P`PB+Ty)ILc>znmi=1+wNtPsNvBEzFzZq<^$h;Y=P&69A} zY}TY7#8hvck3Ts#N_)9be@DE|EWM=HvQl(<^c4ErQR$#k?-AFG|Q9}1)csc0+otFD(9PD$;D>> zB>(&SmH*DpolWm>icL!bwc<}`217mO3jEvaeei&sv;cIU!J=7_cDtCGmJDhk{*E+U zsm3x#Sv2G)9+TSZtle1@Esho`a}B^8u#8wN-Xc3)YF*SW^N{d*3>Gp|^Q1aEM3ODG zTKBSK-Ysy*M(09o*c^f4I@OS+iVI;wRd8Hrp5}t~2Z2$#m*;z93=h0`aUVGa=`jy; zO{CS3Mj+{ievdeFkWR4@FSH78i_`G&wzy!( zSB}`-OqC9dh5y=0!Eycad9CiHy`FeTq*-N!=_|Y^E%4xKW101bFUzLqrzO^1>D5aQ z?&Yhly%8<5I=l~H8(jTY`M)5n7K6`w`BJ8R`pmq6{LSFTd|u6v?6~C?E(Qz*Yd2j` z^!_0H(xk>pY%Yn+44zArKO5@8jiZ=@OyO~=1;HD<$-g{cQ3|!V&BVfHA@dSG4D-+D z^><2AQUWRbI$t*)T8kr}mA(cqI)ckfVJ(Xmx^G_Cq>$irM-6*bRUetUKlREo+A?`h zKuqkeiduwtqlvFT(R*h1?-r#5Fb|1$_#iEp%Jt-R&*wd^mo4ibk}c}LPV>)S{&+uV zn6Py_3?2n~)Q61J=QK`aJR-e&h|~QQ7UNo-PSD)a)~I8UL#yqPtu(a zrDb&Mk0ras!@{1KH!%h!iR>2DGSY5;_KyruTs>I7PDoJd7<9kAi+O>uKm4% zlxbOvhEB9yC9Cm|0bPwUc*;e-B6tSF(nz_7+enL>1-rM>IxSrZO% zvaMnk){=de4YB#AMz-#$L*?<9OWmu>JfTcqm;~g{@lnStt*s^`;Ah^d+19LC#)7`d zvT>TVoMQ(~ncgCR>sQ}jJo>TDhlT%2?Y5Dx5~FVU0JPIAJJDH))Cp6|jB(5K8)hCP za*P4U=!>{=j&895fp*NIvwQ`)=sx$z*g2u%;*|@>IfHv)otd?TT`zG{1hd(0t}+ar zTSflk9*t^7AE8M``=q|adzY)&BUvSdT~gc>-fZ?hQJI6zz4q+slju}jdF=C;5aB~~ ruf^w+SVrB+*4+s(vt0+vj3y^;GRecH2MVFzJrGLTv9)P)(zpKtxpg>M literal 0 HcmV?d00001 diff --git a/sw/ground_segment/python/wind/wind.py b/sw/ground_segment/python/wind/wind.py new file mode 100755 index 00000000000..5d421e42e78 --- /dev/null +++ b/sw/ground_segment/python/wind/wind.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python +# +# Copyright (C) 2018 TUDelft +# +# This file is part of paparazzi. +# +# paparazzi is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# paparazzi is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with paparazzi. If not, see . +# + + +import wx +import wind_frame + +class Wind(wx.App): + def OnInit(self): + self.main = wind_frame.WindFrame() + self.main.Show() + self.SetTopWindow(self.main) + return True + +def main(): + application = Wind(0) + application.MainLoop() + +if __name__ == '__main__': + main() diff --git a/sw/ground_segment/python/wind/wind_frame.py b/sw/ground_segment/python/wind/wind_frame.py new file mode 100644 index 00000000000..1d1fbf302f2 --- /dev/null +++ b/sw/ground_segment/python/wind/wind_frame.py @@ -0,0 +1,210 @@ +# +# Copyright (C) 2018 TUDelft +# +# This file is part of paparazzi. +# +# paparazzi is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# paparazzi is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with paparazzi. If not, see . +# + +import wx + +import sys +import os +import math + +PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../..'))) +PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + '../../../..'))) + +sys.path.append(PPRZ_HOME + "/var/lib/python") + +from pprzlink.ivy import IvyMessagesInterface + +WIDTH = 300.0 +BARH = 40.0 + +MAX_AIRSPEED = 35.0 + +MSG_BUFFER_SIZE = 1000 + +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()