diff --git a/MAVProxy/modules/lib/live_graph_ui.py b/MAVProxy/modules/lib/live_graph_ui.py index 574a28d4ab..62ad1a1879 100644 --- a/MAVProxy/modules/lib/live_graph_ui.py +++ b/MAVProxy/modules/lib/live_graph_ui.py @@ -1,22 +1,25 @@ +from MAVProxy.modules.lib.live_graph import LiveGraph from MAVProxy.modules.lib.wx_loader import wx from MAVProxy.modules.lib import icon import time import numpy, pylab +from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigCanvas +from matplotlib.figure import Figure class GraphFrame(wx.Frame): """ The main frame of the application """ - def __init__(self, state): + def __init__(self, state: LiveGraph): wx.Frame.__init__(self, None, -1, state.title) try: self.SetIcon(icon.SimpleIcon().get_ico()) except Exception: pass self.state = state - self.data = [] - for i in range(len(state.fields)): - self.data.append([]) + self.sample_dt = float(getattr(state, "sample_dt", state.tickresolution)) + self.refresh_dt = float(getattr(state, "refresh_dt", state.tickresolution)) * 10 + self.paused = False self.clear_data = False @@ -26,23 +29,16 @@ def __init__(self, state): self.redraw_timer = wx.Timer(self) self.Bind(wx.EVT_TIMER, self.on_redraw_timer, self.redraw_timer) - self.redraw_timer.Start(int(1000*self.state.tickresolution)) + self.redraw_timer.Start(int(1000*self.refresh_dt)) self.last_yrange = (None, None) def create_main_panel(self): - import platform - if platform.system() == 'Darwin': - from MAVProxy.modules.lib.MacOS import backend_wxagg - FigCanvas = backend_wxagg.FigureCanvasWxAgg - else: - from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigCanvas self.panel = wx.Panel(self) self.init_plot() self.canvas = FigCanvas(self.panel, -1, self.fig) - self.close_button = wx.Button(self.panel, -1, "Close") self.Bind(wx.EVT_BUTTON, self.on_close_button, self.close_button) @@ -72,94 +68,88 @@ def create_main_panel(self): def init_plot(self): self.dpi = 100 - from matplotlib.figure import Figure self.fig = Figure((6.0, 3.0), dpi=self.dpi) self.axes = self.fig.add_subplot(111) - try: - self.axes.set_facecolor('white') - except AttributeError as e: - # this was removed in matplotlib 2.2.0: - self.axes.set_axis_bgcolor('white') - + self.axes.set_facecolor('white') + pylab.setp(self.axes.get_xticklabels(), fontsize=8) pylab.setp(self.axes.get_yticklabels(), fontsize=8) + # create X data + self.max_points = int(self.state.timespan / self.sample_dt) + self.half_points = self.max_points // 2 + self.write_idx = 0 + self.xdata = numpy.arange(-self.state.timespan / 2, self.state.timespan / 2, self.sample_dt)[:self.max_points] + self.axes.set_xbound(lower=self.xdata[0], upper=self.xdata[-1]) + + self.ybuf = [numpy.full(self.max_points, numpy.nan) for _ in range(len(self.state.fields))] + # plot the data as a line series, and save the reference # to the plotted line series # self.plot_data = [] - if len(self.data[0]) == 0: + if len(self.ybuf[0]) == 0: max_y = min_y = 0 else: - max_y = min_y = self.data[0][0] + max_y = min_y = self.ybuf[0][0] num_labels = 0 if not self.state.labels else len(self.state.labels) labels = [] - for i in range(len(self.data)): - if i < num_labels and self.state.labels[i] is not None: + for i in range(len(self.ybuf)): + if i < num_labels and self.state.labels and self.state.labels[i] is not None: label = self.state.labels[i] else: label = self.state.fields[i] labels.append(label) p = self.axes.plot( - self.data[i], + self.xdata, self.ybuf[i], linewidth=1, color=self.state.colors[i%len(self.state.colors)], label=label )[0] self.plot_data.append(p) - if len(self.data[i]) != 0: - min_y = min(min_y, min(self.data[i])) - max_y = max(max_y, max(self.data[i])) + if len(self.ybuf[i]) != 0: + min_y = min(min_y, min(self.ybuf[i])) + max_y = max(max_y, max(self.ybuf[i])) - # create X data - self.xdata = numpy.arange(-self.state.timespan, 0, self.state.tickresolution) - self.axes.set_xbound(lower=self.xdata[0], upper=0) if min_y == max_y: self.axes.set_ybound(min_y, max_y+0.1) self.axes.legend(labels, loc='upper left', bbox_to_anchor=(0, 1.1)) def draw_plot(self): - """ Redraws the plot - """ - state = self.state - - if len(self.data[0]) == 0: - print("no data to plot") + # gather finite values to auto-scale Y + finite_vals = [] + for arr in self.ybuf: + vals = arr[~numpy.isnan(arr)] + if vals.size: + finite_vals.append((vals.min(), vals.max())) + if not finite_vals: + # nothing to show yet return - vhigh = max(self.data[0]) - vlow = min(self.data[0]) - - for i in range(1,len(self.plot_data)): - vhigh = max(vhigh, max(self.data[i])) - vlow = min(vlow, min(self.data[i])) - ymin = vlow - 0.05*(vhigh-vlow) - ymax = vhigh + 0.05*(vhigh-vlow) - - if ymin == ymax: - ymax = ymin + 0.1 * ymin - ymin = ymin - 0.1 * ymin - - if (ymin, ymax) != self.last_yrange: - self.last_yrange = (ymin, ymax) - if ymax == ymin: - ymin = ymin-0.5 - ymax = ymin+1 + vlow = min(lo for lo, hi in finite_vals) + vhigh = max(hi for lo, hi in finite_vals) + if vlow == vhigh: + pad = 0.1 * (abs(vlow) if vlow != 0 else 1.0) + vlow -= pad + vhigh += pad + else: + pad = 0.05 * (vhigh - vlow) + vlow -= pad + vhigh += pad - self.axes.set_ybound(lower=ymin, upper=ymax) - #self.axes.ticklabel_format(useOffset=False, style='plain') + if (vlow, vhigh) != self.last_yrange: + self.last_yrange = (vlow, vhigh) + self.axes.set_ybound(lower=vlow, upper=vhigh) self.axes.grid(True, color='gray') pylab.setp(self.axes.get_xticklabels(), visible=True) - pylab.setp(self.axes.get_legend().get_texts(), fontsize='small') + leg = self.axes.get_legend() + if leg: + pylab.setp(leg.get_texts(), fontsize='small') - for i in range(len(self.plot_data)): - ydata = numpy.array(self.data[i]) - xdata = self.xdata - if len(ydata) < len(self.xdata): - xdata = xdata[-len(ydata):] - self.plot_data[i].set_xdata(xdata) - self.plot_data[i].set_ydata(ydata) + # update lines in place + for i, line in enumerate(self.plot_data): + line.set_ydata(self.ybuf[i]) self.canvas.draw() self.canvas.Refresh() @@ -182,41 +172,61 @@ def on_close_button(self, event): self.Destroy() def on_idle(self, event): - time.sleep(self.state.tickresolution*0.5) + time.sleep(self.refresh_dt*0.5) def on_redraw_timer(self, event): # if paused do not add data, but still redraw the plot # (to respond to scale modifications, grid change, etc.) # + # print("redraw") state = self.state if state.close_graph.wait(0.001): self.redraw_timer.Stop() self.Destroy() return + + batch = [] while state.child_pipe.poll(): - state.values = state.child_pipe.recv() + batch.append(state.child_pipe.recv()) + if self.paused: return if self.clear_data: self.clear_data = False + for i in range(len(self.ybuf)): + self.ybuf[i].fill(numpy.nan) + self.write_idx = 0 + + for msg in batch: + if msg is None: + print("msg is none") + continue + + if self.write_idx >= self.max_points: + # shift the right half to the left half, clear the right half + for i in range(len(self.ybuf)): + self.ybuf[i][:self.half_points] = self.ybuf[i][self.half_points:] + self.ybuf[i][self.half_points:] = numpy.nan + self.write_idx = self.half_points + for i in range(len(self.plot_data)): - if state.values[i] is not None: - while len(self.data[i]): - self.data[i].pop(0) + val = msg[i] if i < len(msg) else None + if isinstance(val, list): + print("ERROR: Cannot plot array of length %d. Use 'graph %s[index]' instead"%(len(val), state.fields[i])) + self.redraw_timer.Stop() + self.Destroy() + return + if val is not None: + self.ybuf[i][self.write_idx] = val + + # write once per message, not once per field + self.write_idx += 1 - for i in range(len(self.plot_data)): - if (type(state.values[i]) == list): - print("ERROR: Cannot plot array of length %d. Use 'graph %s[index]' instead"%(len(state.values[i]), state.fields[i])) - self.redraw_timer.Stop() - self.Destroy() - return - if state.values[i] is not None: - self.data[i].append(state.values[i]) - while len(self.data[i]) > len(self.xdata): - self.data[i].pop(0) for i in range(len(self.plot_data)): - if state.values[i] is None or len(self.data[i]) < 2: + if len(self.ybuf[i]) < 2: + print("return from here") return + self.draw_plot() diff --git a/MAVProxy/modules/lib/new_graph_ui.py b/MAVProxy/modules/lib/new_graph_ui.py new file mode 100644 index 0000000000..53cff00ada --- /dev/null +++ b/MAVProxy/modules/lib/new_graph_ui.py @@ -0,0 +1,280 @@ +# from MAVProxy.modules.lib.wx_loader import wx +# from MAVProxy.modules.lib import icon +# import time +# import math +# from collections import deque +# import numpy as np + +# class GraphFrame(wx.Frame): +# """ +# wxPython frame embedding a Matplotlib plot that: +# - starts with a time window including +future_pad_s seconds into the future, +# - fills points left→right without moving the x-axis, +# - when the right edge is reached, shifts the window forward by page_step_s, +# keeping the x-span constant. +# """ + +# def __init__(self, state): +# print("ZARI: GraphFrame Constructor") +# wx.Frame.__init__(self, None, -1, state.title) +# try: +# self.SetIcon(icon.SimpleIcon().get_ico()) +# except Exception: +# pass + +# # ====== config/state ====== +# self.state = state +# self.tick_s = float(getattr(state, "tickresolution", 0.1)) # seconds between timer ticks +# self.span_s = float(getattr(state, "timespan", 30.0)) # total visible x-span +# self.future_pad_s = float(getattr(state, "future_pad_s", 10.0))# initial +future +# self.page_step_s = float(getattr(state, "page_step_s", 5.0)) # jump size when reaching right edge + +# n_series = len(state.fields) +# self.t_buf = deque() # timestamps (epoch seconds) +# self.y_bufs = [deque() for _ in range(n_series)] # one deque per series + +# # window_start/end are absolute timestamps +# now = time.time() +# self.x_end = now + self.future_pad_s +# self.x_start = self.x_end - self.span_s + +# self.paused = False +# self.clear_requested = False +# self.last_yrange = (None, None) + +# self.create_main_panel() + +# # drive updates with a timer +# self.redraw_timer = wx.Timer(self) +# self.Bind(wx.EVT_TIMER, self.on_redraw_timer, self.redraw_timer) +# self.redraw_timer.Start(int(1000 * self.tick_s)) + +# # ---------- UI ---------- + +# def create_main_panel(self): +# print("ZARI: create_main_panel") +# import platform +# if platform.system() == 'Darwin': +# from MAVProxy.modules.lib.MacOS import backend_wxagg +# FigCanvas = backend_wxagg.FigureCanvasWxAgg +# else: +# from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigCanvas + +# from matplotlib.figure import Figure + +# self.panel = wx.Panel(self) + +# # Figure/Axes +# self.dpi = 100 +# self.fig = Figure((6.0, 3.0), dpi=self.dpi) +# self.axes = self.fig.add_subplot(111) +# # facecolor compat +# if hasattr(self.axes, "set_facecolor"): +# self.axes.set_facecolor("white") +# else: +# self.axes.set_axis_bgcolor("white") + +# # pre-create lines (empty) for each series +# self.lines = [] +# labels = [] +# num_labels = len(getattr(self.state, "labels", []) or []) +# for i in range(len(self.y_bufs)): +# label = self.state.labels[i] if (i < num_labels and self.state.labels[i]) else self.state.fields[i] +# color = self.state.colors[i % len(self.state.colors)] +# line, = self.axes.plot([], [], linewidth=1, color=color, label=label) +# self.lines.append(line) +# labels.append(label) + +# self.axes.legend(labels, loc='upper left', bbox_to_anchor=(0, 1.1)) +# for lbl in self.axes.get_xticklabels() + self.axes.get_yticklabels(): +# lbl.set_fontsize(8) + +# # set initial x-limits (absolute time in seconds) +# self.axes.set_xbound(lower=self.x_start, upper=self.x_end) +# self.axes.grid(True) + +# # Canvas +# self.canvas = FigCanvas(self.panel, -1, self.fig) + +# # Controls +# self.close_button = wx.Button(self.panel, -1, "Close") +# self.pause_button = wx.Button(self.panel, -1, "Pause") +# self.clear_button = wx.Button(self.panel, -1, "Clear") + +# self.Bind(wx.EVT_BUTTON, self.on_close_button, self.close_button) +# self.Bind(wx.EVT_BUTTON, self.on_pause_button, self.pause_button) +# self.Bind(wx.EVT_BUTTON, self.on_clear_button, self.clear_button) +# self.Bind(wx.EVT_UPDATE_UI, self.on_update_pause_button, self.pause_button) + +# # Layout +# hbox = wx.BoxSizer(wx.HORIZONTAL) +# for i, button in enumerate((self.close_button, self.pause_button, self.clear_button)): +# if i > 0: +# hbox.AddSpacer(5) +# hbox.Add(button, 0, wx.ALL | wx.ALIGN_CENTER_VERTICAL, 5) + +# vbox = wx.BoxSizer(wx.VERTICAL) +# vbox.Add(self.canvas, 1, flag=wx.LEFT | wx.TOP | wx.GROW) +# vbox.Add(hbox, 0, flag=wx.ALIGN_LEFT | wx.TOP) + +# self.panel.SetSizer(vbox) +# vbox.Fit(self) + +# # ---------- Data & drawing ---------- + +# def _append_sample(self, t_epoch, values): +# """Append one sample (timestamp + per-series values).""" +# # initialize deques' maxlen lazily once we know effective capacity +# # capacity ~= enough points to cover span_s at tick_s cadence, plus a small headroom +# if getattr(self, "_maxlen", None) is None: +# cap = int(math.ceil(self.span_s / self.tick_s)) + 64 +# self._maxlen = cap +# self.t_buf = deque(maxlen=cap) +# self.y_bufs = [deque(maxlen=cap) for _ in self.y_bufs] + +# self.t_buf.append(t_epoch) +# for i, y in enumerate(self.y_bufs): +# val = values[i] +# if isinstance(val, list): +# # same guard as before +# raise ValueError( +# f"Cannot plot array of length {len(val)} for {self.state.fields[i]}. " +# f"Use 'graph {self.state.fields[i]}[index]' instead." +# ) +# y.append(val) + +# def _visible_slice(self): +# """Return numpy arrays (tx, ys) filtered to current [x_start, x_end] window.""" +# if not self.t_buf: +# return None, None + +# # Convert to numpy for fast slicing +# tx = np.fromiter(self.t_buf, dtype=float) +# mask = (tx >= self.x_start) & (tx <= self.x_end) +# if not mask.any(): +# return None, None + +# txv = tx[mask] +# ysv = [] +# for ybuf in self.y_bufs: +# y = np.fromiter(ybuf, dtype=float) +# ysv.append(y[mask]) +# return txv, ysv + +# def _maybe_advance_window(self, latest_t): +# """ +# If the newest timestamp reached/passed the right edge, +# jump the window forward by page_step_s (keeping span constant). +# """ +# if latest_t >= self.x_end: +# self.x_start += self.page_step_s +# self.x_end += self.page_step_s +# self.axes.set_xbound(lower=self.x_start, upper=self.x_end) + +# def _autoscale_y_from_visible(self, ysv): +# """Autoscale Y based on currently visible data.""" +# vals = np.concatenate([y for y in ysv if y.size > 0]) if ysv else np.array([]) +# if vals.size == 0: +# return +# vmin = float(np.nanmin(vals)) +# vmax = float(np.nanmax(vals)) +# if not np.isfinite(vmin) or not np.isfinite(vmax): +# return +# if vmin == vmax: +# pad = 0.1 if vmin == 0 else abs(vmin) * 0.1 +# vmin -= pad +# vmax += pad +# # avoid excessive ybound churn +# if (vmin, vmax) != self.last_yrange: +# self.last_yrange = (vmin, vmax) +# self.axes.set_ybound(lower=vmin, upper=vmax) + +# def draw_plot(self): +# """Update line data and request a draw (no axis motion unless paging).""" +# tx, ysv = self._visible_slice() +# if tx is None: +# self.canvas.draw_idle() +# return + +# for line, y in zip(self.lines, ysv): +# line.set_xdata(tx) +# line.set_ydata(y) + +# self._autoscale_y_from_visible(ysv) +# self.canvas.draw_idle() + +# # ---------- Event handlers ---------- + +# def on_pause_button(self, event): +# self.paused = not self.paused + +# def on_update_pause_button(self, event): +# self.pause_button.SetLabel("Resume" if self.paused else "Pause") + +# def on_clear_button(self, event): +# self.clear_requested = True + +# def on_close_button(self, event): +# self.redraw_timer.Stop() +# self.Destroy() + +# def on_redraw_timer(self, event): +# """Ingest data from the pipe, page x-window when needed, and redraw.""" +# state = self.state + +# # external close +# if state.close_graph.wait(0.001): +# self.redraw_timer.Stop() +# self.Destroy() +# return + +# # drain pipe to latest values +# while state.child_pipe.poll(): +# state.values = state.child_pipe.recv() + +# if self.paused: +# # still allow the plot to redraw (e.g., window paging on resume) +# self.draw_plot() +# return + +# # handle clear request +# if self.clear_requested: +# self.clear_requested = False +# self.t_buf.clear() +# for y in self.y_bufs: +# y.clear() + +# # ingest one sample if available +# vals = getattr(state, "values", None) +# if vals is None: +# return +# # require at least one non-None series; keep alignment by appending None as NaN +# now = time.time() +# try: +# cleaned = [] +# for v in vals: +# if v is None: +# cleaned.append(np.nan) +# elif isinstance(v, list): +# # mirror original behavior (error and close) but raise to keep logic tidy +# raise ValueError +# else: +# cleaned.append(float(v)) +# self._append_sample(now, cleaned) +# except ValueError: +# print(f"ERROR: Cannot plot array value. Use 'graph field[index]' instead.") +# self.redraw_timer.Stop() +# self.Destroy() +# return + +# # page window if we've reached the right edge +# self._maybe_advance_window(now) + +# # draw +# # only draw when we have >= 2 points visible for each active series +# tx, ysv = self._visible_slice() +# if tx is None: +# return +# if any(y.size < 2 for y in ysv): +# return +# self.draw_plot() diff --git a/README.md b/README.md index 0dd82f3280..b6ce362521 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,59 @@ +Installation: + +sudo apt install -y python3-wxgtk4.0 python3-matplotlib +python3 -m venv --system-site-packages venv +pip install -e .[recommended] + + +#### + +First run: + +run ardupilot simulation in another console, eg: + +```sim_vehicle.py -v ArduCopter --no-mavproxy``` + +check on which port the SITL is runnning, and then run mavproxy, eg: + +```mavproxy.py --master=tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --map``` + +For take off: +mode GUIDED +arm throttle +takeoff 10 + +wp clear +wp add -35.3632636 149.1662277 20 +wp add -35.3632636 149.1666691 20 +wp add -35.3629518 149.1664484 20 +wp list +mode AUTO + + +In order to run graph, do eg.: + +``` +module load graph +graph ATTITUDE.roll ATTITUDE.pitch ATTITUDE.yaw +graph VFR_HUD.climb # Vertical speed (positive up) +``` + +In order to make changes visible in the code: +``` +module unload graph +module load graph +``` + +to get list of possible values type: +```status``` + +to change tickresolution / refresh rate of the graph do: + +```module tickresolution ``` + +where value is refresh period in seconds + + ![GitHub Actions](https://github.com/ardupilot/MAVProxy/actions/workflows/windows_build.yml/badge.svg) MAVProxy