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 00000000000..d1b53be0fb2
Binary files /dev/null and b/sw/ground_segment/python/wind/wind.png differ
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()