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

@@ -69,6 +69,8 @@
     <program name="SVInfo" command="sw/ground_segment/python/svinfo/svinfo.py"/>
     <program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>
     <program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
+    <program name="Wind" command="sw/ground_segment/python/wind/wind.py"/>
+    <program name="EnergyManager" command="sw/ground_segment/python/energy_mon/energy_mon.py"/>
     <program name="AeronauticalInfo" command="sw/ground_segment/python/atc/atc.py"/>
     <program name="RtpViewer" command="sw/tools/rtp_viewer/rtp_viewer.py">
       <arg flag="-p" constant="5000"/>

conf/userconf/tudelft/control_panel.xml

@@ -59,6 +59,8 @@
     <program name="ADS-B Intruders receiver" command="sw/ground_segment/misc/sbs2ivy">
       <arg flag="--ac" constant="@AC_ID"/>
     </program>
+    <program name="ATC" command="sw/ground_segment/python/atc/atc.py"/>
+    <program name="Wind" command="sw/ground_segment/python/wind/wind.py"/>
     <program name="SVInfo" command="sw/ground_segment/python/svinfo/svinfo.py"/>
     <program name="EnergyManager" command="sw/ground_segment/python/energy_mon/energy_mon.py"/>
     <program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>

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()

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))