diff --git a/dronecan_gui_tool/panels/esc_panel.py b/dronecan_gui_tool/panels/esc_panel.py index f69de6e..fd6e3cf 100644 --- a/dronecan_gui_tool/panels/esc_panel.py +++ b/dronecan_gui_tool/panels/esc_panel.py @@ -15,6 +15,7 @@ from ..widgets import make_icon_button, get_icon, get_monospace_font import sip import time +import math __all__ = 'PANEL_NAME', 'spawn', 'get_icon' @@ -93,16 +94,21 @@ def view_mode_set_value(self, value): self._slider.setValue(value) self._spinbox.setValue(value) - def update_status(self, msg): - status = msg.message - if status.esc_index == self._index: - self._error_count_label.setText(f'Err: {status.error_count}') - self._voltage_label.setText(f'Volt: {status.voltage:.1f} V') - self._current_label.setText(f'Curr: {status.current:.1f} A') - temperature_celsius = status.temperature - 273.15 + def update_status(self, idx, error_count, voltage, current, temperature_celsius, rpm, power_rating_pct): + if idx != self._index: + return + if error_count is not None: + self._error_count_label.setText(f'Err: {error_count}') + if voltage is not None: + self._voltage_label.setText(f'Volt: {voltage:.1f} V') + if current is not None: + self._current_label.setText(f'Curr: {current:.1f} A') + if temperature_celsius is not None: self._temperature_label.setText(f'Temp: {temperature_celsius:.1f} °C') - self._rpm_label.setText(f'RPM: {status.rpm}') - self._power_rating_pct_label.setText(f'RAT: {status.power_rating_pct} %') + if rpm is not None: + self._rpm_label.setText(f'RPM: {rpm}') + if power_rating_pct is not None: + self._power_rating_pct_label.setText(f'RAT: {power_rating_pct} %') class ESCPanel(QDialog): @@ -118,6 +124,8 @@ def __init__(self, parent, node): self.setAttribute(Qt.WA_DeleteOnClose) # This is required to stop background timers! self._node = node + self.send_hobbywing = False + self.hobbywing_map = {} self._view_mode = QCheckBox(self) self._view_mode_label = QLabel('View Mode Inactive', self) @@ -195,14 +203,54 @@ def __init__(self, parent, node): self.setLayout(layout) self.resize(self.minimumWidth(), self.minimumHeight()) - self._node.add_handler(dronecan.uavcan.equipment.esc.Status, self._on_esc_status) - self._node.add_handler(dronecan.uavcan.equipment.esc.RawCommand, self._on_esc_raw_command) - + self.handlers = [self._node.add_handler(dronecan.uavcan.equipment.esc.Status, self._on_esc_status), + self._node.add_handler(dronecan.com.hobbywing.esc.StatusMsg1, self.handle_StatusMsg1), + self._node.add_handler(dronecan.com.hobbywing.esc.StatusMsg2, self.handle_StatusMsg2), + self._node.add_handler(dronecan.com.hobbywing.esc.GetEscID, self.handle_GetEscID), + self._node.add_handler(dronecan.uavcan.equipment.esc.RawCommand, self._on_esc_raw_command)] + def _on_esc_status(self, msg): if msg.message.esc_index < len(self._sliders): sl = self._sliders[msg.message.esc_index] if sl and not sip.isdeleted(sl): - sl.update_status(msg) + status = msg.message + temperature_celsius = status.temperature - 273.15 + sl.update_status(status.esc_index, status.error_count, status.voltage, status.current, + temperature_celsius, status.rpm, status.power_rating_pct) + + def handle_StatusMsg1(self, msg): + '''handle Hobbywing StatusMsg1''' + if not self.send_hobbywing: + print("Detected HobbyWing ESC") + self.send_hobbywing = True + nodeid = msg.transfer.source_node_id + escid = self.hobbywing_map.get(nodeid,None) + if escid is None: + return + sl = self._sliders[escid] + if sl and not sip.isdeleted(sl): + sl.update_status(escid, None, None, None, + None, msg.message.rpm, None) + + def handle_StatusMsg2(self, msg): + '''handle Hobbywing StatusMsg2''' + nodeid = msg.transfer.source_node_id + escid = self.hobbywing_map.get(nodeid,None) + if escid is None: + return + sl = self._sliders[escid] + if sl and not sip.isdeleted(sl): + sl.update_status(escid, None, msg.message.input_voltage*0.1, msg.message.current*0.1, + msg.message.temperature, None, None) + + def handle_GetEscID(self, msg): + '''handle HobbyWing GetEscID''' + if len(msg.message.payload) != 2: + # likely from flight controller + return + nodeid = msg.transfer.source_node_id + escid = msg.message.payload[1] + self.hobbywing_map[nodeid] = escid-1 def _on_esc_raw_command(self, msg): if msg.transfer.source_node_id is not self._node.node_id: @@ -260,8 +308,18 @@ def _do_broadcast(self): self._node.broadcast(msg) self._msg_viewer.setPlainText(dronecan.to_yaml(msg)) + if self.send_hobbywing: + # also handle HobbyWing ESCs + msg = dronecan.com.hobbywing.esc.RawCommand() + for sl in self._sliders: + raw_value = sl.get_value() / 100 + value = (-self.CMD_MIN if raw_value < 0 else self.CMD_MAX) * raw_value + msg.command.append(int(value)) + self._node.broadcast(msg) else: self._msg_viewer.setPlainText('Paused') + else: + self._msg_viewer.setPlainText('Paused') except Exception as ex: self._msg_viewer.setPlainText('Publishing failed:\n' + str(ex)) @@ -293,6 +351,8 @@ def deferred_resize(): def __del__(self): global _singleton + for h in self.handlers: + h.remove() _singleton = None def closeEvent(self, event): diff --git a/dronecan_gui_tool/panels/hobbywing_esc.py b/dronecan_gui_tool/panels/hobbywing_esc.py index 465fd5e..e798f47 100644 --- a/dronecan_gui_tool/panels/hobbywing_esc.py +++ b/dronecan_gui_tool/panels/hobbywing_esc.py @@ -35,6 +35,7 @@ def __init__(self, parent, node): self.setAttribute(Qt.WA_DeleteOnClose) # This is required to stop background timers! self._node = node + self.node_map = {} layout = QVBoxLayout() @@ -103,6 +104,16 @@ def __init__(self, parent, node): self.msg3rate_set.clicked.connect(self.on_msg3rate_set) layout.addLayout(self.labelWidget('Msg3Rate:', [self.msg3rate, self.msg3rate_set])) + + self.throttle_source = QComboBox(self) + self.throttle_source.addItem("---") + self.throttle_source.addItem("PWM") + self.throttle_source.addItem("CAN") + self.throttle_source.setCurrentText("---") + self.throttle_source_set = QPushButton('Set', self) + self.throttle_source_set.clicked.connect(self.on_throttle_source_set) + + layout.addLayout(self.labelWidget('Throttle_source:', [self.throttle_source, self.throttle_source_set])) self.setLayout(layout) self.resize(400, 200) @@ -121,6 +132,23 @@ def handle_reply(self, msg): else: print("No reply") + def handle_throttle_source_reply(self, msg): + '''handle a reply to a set of throttle source''' + if msg is not None: + print('REPLY: ', dronecan.to_yaml(msg)) + else: + print("No reply") + + def handle_GetMajorConfig_reply(self, msg): + '''handle a reply to a GetMajorConfig''' + nodeid = msg.transfer.source_node_id + if msg.response.throttle_source == 0: + txt = "CAN" + else: + txt = "PWM" + if self.throttle_source.currentText() == "---": + self.throttle_source.setCurrentText(txt) + def on_throttleid_set(self): '''set throttle ID''' nodeid = self.table.get_selected() @@ -161,6 +189,16 @@ def on_direction_set(self): req.direction = 0 if self.direction.currentText() == "CW" else 1 self._node.request(req, nodeid, self.handle_reply) + def on_throttle_source_set(self): + '''set throttle source''' + nodeid = self.table.get_selected() + req = dronecan.com.hobbywing.esc.SetThrottleSource.Request() + txt = self.throttle_source.currentText() + if txt == "---": + return + req.source = 0 if txt == "CAN" else 1 + self._node.request(req, nodeid, self.handle_throttle_source_reply) + def set_rate(self, nodeid, msgid, rate): '''set a message rate''' req = dronecan.com.hobbywing.esc.SetReportingFrequency.Request() @@ -205,6 +243,7 @@ def handle_GetEscID(self, msg): data = self.table.get(nodeid) if data is None: data = [nodeid, 0, 0, 0, 0, 0, 0] + self.node_map[nodeid] = msg.message.payload[1] data[1] = msg.message.payload[1] self.table.update(nodeid, data) @@ -235,6 +274,12 @@ def request_ids(self): req = dronecan.com.hobbywing.esc.GetEscID() req.payload = [0] self._node.broadcast(req) + # for IDs we have, send GetMajorConfig + for nodeid in self.node_map.keys(): + req = dronecan.com.hobbywing.esc.GetMajorConfig.Request() + req.option = 0 + self._node.request(req, nodeid, self.handle_GetMajorConfig_reply) + def labelWidget(self, label, widgets): '''a widget with a label'''