Upload files to 'src/fc_network_adapter/fc_network_adapter'

src/fc_network_adapter/fc_network_adapter/mavlinkPublish.py

@@ -9,6 +9,8 @@
 publisher topic name 命名規則為 <前綴詞>/s<sysid>/<具體 topic>
 '''
 
+from pymavlink import mavutil
+
 import std_msgs.msg
 import sensor_msgs.msg
 import geometry_msgs.msg
@@ -24,13 +26,13 @@ class mavlink_publisher():
 
     prefix_path = 'MavLinkBus'
 
-    def create_flightMode(self, sysid, component_obj):
+    def create_state(self, sysid, component_obj):
         # target topic name # 請跟這個 method 的名稱保持一致
-        target_topic = 'flightMode'
+        target_topic = 'state'
 
         # 這邊要檢查 flight_mode 是否存在
         try:
-            _ = component_obj.emitParams['flightMode_mode']
+            _ = component_obj.emitParams['heartbeat']
         except KeyError:
             # 這個 component id 還不存在
             logger.warning('System ID : {} This Component ID : {} Did not init yet'.format(component_obj['sysid'], component_obj['compid']))
@@ -38,15 +40,16 @@ class mavlink_publisher():
         
         # 若存在則 建立 publisher object 並回傳 true
         topic_name = '{}/s{}/{}'.format(self.prefix_path, sysid, target_topic)
-        publisher_ = self.create_publisher(std_msgs.msg.String, topic_name, 1)
-        component_obj.publishers[target_topic] = [publisher_, self.packEmit_flightMode]
+        publisher_ = self.create_publisher(mavros_msgs.msg.State, topic_name, 1)
+        component_obj.publishers[target_topic] = [publisher_, self.packEmit_state]
 
         return True
 
-    def packEmit_flightMode(cls, emitParams, publisher):
-        msg_str = emitParams['flightMode_mode']
-        msg = std_msgs.msg.String()
-        msg.data = msg_str
+    def packEmit_state(cls, emitParams, publisher):
+        mav_msg = emitParams['heartbeat']
+        msg = mavros_msgs.msg.State()
+        msg.mode = mavutil.mode_string_v10(mav_msg)
+        msg.armed = (mav_msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
         publisher.publish(msg)
         pass