uavlink update

class_model/src/uavlinkPubMain.py

@@ -21,15 +21,16 @@ from geometry_msgs.msg import Vector3
 
 
 def init_dataFormat(cfg:Read_PUB_Config):
-    ros_namespace="/drone1"
-    f1data = b'\xf1drone550..............\r\n'  
-    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
+    f1data = b'\xf1drone550pub...........\r\n'
+    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout=0.3)
     sel.write(f1data)
+    readTenByte = sel.readline()
+    print(readTenByte)
     Proto_msg_from_ros.sel = sel
     Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()
 
-    rospy.Subscriber(ros_namespace+'/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps)
-    rospy.Subscriber(ros_namespace+'/mavros/global_position/compass_hdg', Float64, Proto_msg_from_ros.callBack_compass_hdg)
+    rospy.Subscriber('/mavros/global_position/global', NavSatFix, Proto_msg_from_ros.callBack_gps)
+    rospy.Subscriber('/mavros/global_position/compass_hdg', Float64, Proto_msg_from_ros.callBack_compass_hdg)
 
 if __name__ == '__main__':
     FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_PUB.yml")
@@ -47,7 +48,7 @@ if __name__ == '__main__':
     file_handler.setFormatter(formatter)
     file_handler.setLevel(logging.INFO)
 
-    logger = logging.getLogger(__name__)
+    logger = logging.getLogger("__UAVLINKSUBPUB__")
     logger.setLevel(logging.INFO)
     logger.addHandler(file_handler)
     logger.addHandler(stream_handler)
@@ -72,3 +73,4 @@ if __name__ == '__main__':
 
 
 
+

class_model/src/uavlinkSubMain.py

@@ -22,22 +22,24 @@ from geometry_msgs.msg import Vector3
 
 def init_dataFormat(cfg:Read_SUB_Config):
     ros_namespace="/drone1"
-    f1data = b'\xf1drone550..............\r\x1a'  
+    deviceData = b'\xf12222222222222222222222\r\x1a'
 
-    sel.write(f1data)
-    readTenByte = sel.read_until(size=5)
+    sel.write(deviceData)
+    time.sleep(1)
+    readTenByte = sel.read(size=5)
     print(readTenByte)
 
+
     Proto_msg_to_ros.flight_information_msg = flight_information_pb2.flight_information_message()
     Proto_msg_to_ros.rate = rospy.Rate(10)
-    Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.ROStopicName_Flight_Information,String,queue_size=10)
+    Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
 
     
 if __name__ == '__main__':
     FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml")
     cfg = Read_SUB_Config(FilePath)
-    print(cfg)
-    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
+    
+    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout = 0.5)
     # set log
     log_format = "%(asctime)s - %(levelname)s - %(message)s"
     formatter = logging.Formatter(log_format)
@@ -50,20 +52,34 @@ if __name__ == '__main__':
     file_handler.setFormatter(formatter)
     file_handler.setLevel(logging.INFO)
 
-    logger = logging.getLogger(__name__)
+    logger = logging.getLogger("__UAVLINKSUB__")
     logger.setLevel(logging.INFO)
     logger.addHandler(file_handler)
     logger.addHandler(stream_handler)
     logger.info(cfg)
     
+
+    rospy.init_node(cfg.ROSClientNameSub)  
+    init_dataFormat(cfg)
+ 
+
     while True:
         try:
             # test json
-            readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
+            if sel.in_waiting >= 25:
+                readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
+                # print(readTenByte)
+                if b"drone550" in readTenByte:
+                    print(readTenByte)
+                    print("radar")
+                    continue
                 
+                Proto_msg_to_ros.on_message_Flight_Information(readTenByte)
 
-        except KeyboardInterrupt as e:
+        except Exception as e:
             print("End of program")
+            print(readTenByte)
+            pass
 
 
 

class_model/src/utils/proto_uavlink_pub_data_to_ros.py

@@ -10,7 +10,6 @@ logger = logging.getLogger("__SUB__")
 class Proto_msg_to_ros:
     #Protobuf
     flight_information_msg = None
-    sel = None
 
     #Ros publisher
     rate = None
@@ -20,7 +19,8 @@ class Proto_msg_to_ros:
     #Proto
     @classmethod
     def on_message_Flight_Information(cls, msg):
-        proto = msg[1:23]
+        proto = msg[1:-2]
+
         proto_msg = cls.flight_information_msg.FromString(proto)
         protoTOJson_msg = json_format.MessageToJson(proto_msg, indent=None, preserving_proto_field_name=True)
         logger.info(protoTOJson_msg)
@@ -28,5 +28,3 @@ class Proto_msg_to_ros:
         cls.rate.sleep()
 
         
-
-

class_model/src/utils/proto_uavlink_sub_data_from_ros.py

@@ -23,5 +23,6 @@ class Proto_msg_from_ros:
         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.read_all()
+        print(len(b'\xf2' + flightInformationMsg + b'\r\n'))
+        readTenByte = cls.sel.readline()
         print(readTenByte)

class_model/src/utils/uavlinkConfig_PUB.yml

@@ -2,7 +2,7 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
-  ttyport: /dev/ttyUSB0
+  ttyport: /dev/ttyTHS1
 MQTT: "None"
 #ROS
 ROS:

class_model/src/utils/uavlinkConfig_SUB.yml

@@ -2,11 +2,10 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
-  ttyport: /dev/ttyUSB2
+  ttyport: /dev/ttyTHS1
 MQTT: "None"
 ROS:
   ROSClientNameSub: Drone550UAVLINKSub
-  ROStopicName_Flight_Information: Flight_Information_reciver
-  ROStopicName_Fly_Formation: Fly_Formation_reciver
+  Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
 LOG:
   logFileName: UAVLINKsub.log