uavlink with ros no check

class_model/src/uavlinkPubMain.py

@@ -0,0 +1,71 @@
+import serial
+import time
+import random
+import utils
+import sys
+import os
+import proto.flight_information_pb2 as flight_information_pb2
+import logging
+
+import rospy
+from std_msgs.msg import String
+from std_msgs.msg import Float64
+from std_msgs.msg import Header
+from mavros_msgs.srv import ParamGet
+from sensor_msgs.msg import NavSatFix
+from sensor_msgs.msg import Imu
+from sensor_msgs.msg import Range
+from geometry_msgs.msg import Vector3
+
+
+def init_dataFormat(cfg:utils.Read_PUB_Config):
+    ros_namespace="/drone1"
+    f1data = b'\xf1drone550..............\r\n'  
+    sel = serial.Serial(cfg.port, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
+    sel.write(f1data)
+    utils.Proto_msg_from_ros.sel = sel
+
+    rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, utils.Proto_msg_from_ros.callBack_gps)
+    rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, utils.Proto_msg_from_ros.callBack_compass_hdg)
+
+if __name__ == '__main__':
+    FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB.yml")
+    cfg = utils.Read_PUB_Config(FilePath)
+
+    # set log
+    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    formatter = logging.Formatter(log_format)
+
+    stream_handler = logging.StreamHandler()
+    stream_handler.setFormatter(formatter)
+    stream_handler.setLevel(logging.DEBUG)
+
+    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler.setFormatter(formatter)
+    file_handler.setLevel(logging.INFO)
+
+    logger = logging.getLogger(__name__)
+    logger.setLevel(logging.INFO)
+    logger.addHandler(file_handler)
+    logger.addHandler(stream_handler)
+    logger.info(cfg)    
+
+
+
+    # Ros
+    rosClient = cfg.ROSClientNamePub
+    rospy.init_node(rosClient)
+    
+    # init data format
+    init_dataFormat(cfg)
+
+    try:
+        rospy.spin()
+    except BaseException as error:
+        logger.info("End of program")
+        sys.exit()
+     
+
+
+
+

class_model/src/utils/proto_uavlink_sub_data_from_ros.py

@@ -0,0 +1,27 @@
+import orjson
+import time
+import logging
+
+# logger = logging.getLogger("__PUB__")
+
+class Proto_msg_from_ros:
+    #Protobuf
+    flight_information_msg = None
+    sel = None
+
+
+    @classmethod
+    def callBack_gps(cls, GPS):
+        cls.flight_information_msg.gps.LAT = GPS.latitude
+        cls.flight_information_msg.gps.LON = GPS.longitude
+        cls.flight_information_msg.gps.ALT = GPS.altitude
+        # print ('lat:'+lat+'\n'+'lon:'+lon+'\n')
+        # print(cls.flight_information_msg)
+
+    @classmethod
+    def callBack_compass_hdg(cls, Compass):
+        cls.flight_information_msg.heading = Compass.data
+        flightInformationMsg = cls.flight_information_msg.SerializeToString()
+        cls.sel.write(b'\xf2' + flightInformationMsg + b'\r\n')
+        readTenByte = cls.sel.readline()
+        print(readTenByte)

class_model/src/utils/readConfig.py

@@ -2,7 +2,7 @@ import yaml
 
 class Config:
     def __init__(self, inFileName):
-        self.sectionNames = ["MQTT","ROS", "LOG"]
+        self.sectionNames = ["MQTT","ROS", "LOG", "UAVLINK"]
         self.options = {}
         self.inFileName = inFileName
 
@@ -43,24 +43,29 @@ class Read_PUB_Config(Config):
         super().__init__(inFileName)  
         self.options = {
             self.sectionNames[0]:{
-                            "msg_format": (str,True),
+                            "msg_format": (str,False),
-                            "MQTTClientNamePub": (str,True),
+                            "MQTTClientNamePub": (str,False),
-                            "host": (str,True),
+                            "host": (str,False),
-                            "port": (int,True),
+                            "port": (int,False),
-                            "keepalive": (int,True),
+                            "keepalive": (int,False),
-                            "willTopic":(str,True),
+                            "willTopic":(str,False),
-                            "lwt":(str, True),
+                            "lwt":(str, False),
                             "willRetain":(bool,False),
-                            "willTopicQOS":(int,True),
+                            "willTopicQOS":(int,False),
                             "Flight_Information_topicToMqtt": (str,False),
                             "Fly_Formation_topicToMqtt": (str,False),
                             "Fly_Formation_topicToMqtt_QOS":(int,False)},
             self.sectionNames[1]:{
-                            "ROSClientNamePub": (str,False),
+                            "ROSClientNamePub": (str,True),
                             "ROStopicName_Flight_Information": (str,False),
                             "ROStopicName_Fly_Formation": (str,False)},
             self.sectionNames[2]:{
-                            "logFileName":(str,False)}}
+                            "logFileName":(str,True)},
+            self.sectionNames[3]:{
+                            "msg_format": (str,False),
+                            "uav_id": (str,False),
+                            "baudrate": (int,False),
+                            "port": (str,False)}}
         self.setAttribute()
 
     def __str__(self):

class_model/src/utils/uavlinkConfig_PUB.yml

@@ -0,0 +1,12 @@
+UAVLINK:
+  msg_format: Proto
+  uav_id: \x01\x01
+  baudrate: 250000
+  port: /dev/ttyUSB2
+MQTT: "None"
+#ROS
+ROS:
+  ROSClientNamePub: Drone550UAVLINKPub
+  ROStopicName_Flight_Information: Flight_Information_reciver
+LOG:
+  logFileName: UAVLINKpub.log