hobbywing: support ESC panel and add throttle source control

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

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