test bio drones

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)
@@ -67,6 +68,7 @@ if __name__ == '__main__':
     except BaseException as error:
         logger.info("End of program")
         sys.exit()
+
      
 
 

class_model/src/uavlinkSubMain.py

@@ -22,12 +22,21 @@ from geometry_msgs.msg import Vector3
 
 def init_dataFormat(cfg:Read_SUB_Config):
     ros_namespace="/drone1"
+<<<<<<< HEAD
     f1data = b'\xf1drone550..............\r\x1a'
     
     sel.write(f1data)
     readTenByte = sel.read_until(size=5)
+=======
+    deviceData = b'\xf12222222222222222222222\r\x1a'
+
+    sel.write(deviceData)
+    time.sleep(1)
+    readTenByte = sel.read(size=5)
+>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
     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.Dron550_ROStopicName_Flight_Information,String,queue_size=10)
@@ -36,8 +45,8 @@ def init_dataFormat(cfg:Read_SUB_Config):
 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,7 +59,7 @@ 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)
@@ -59,6 +68,10 @@ if __name__ == '__main__':
 
     rospy.init_node(cfg.ROSClientNameSub)  
     init_dataFormat(cfg)
+<<<<<<< HEAD
+=======
+ 
+>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
 
     flight_information_msg = flight_information_pb2.flight_information_message()
     flight_information_msg.gps.LAT = 34123.1231515
@@ -70,13 +83,30 @@ if __name__ == '__main__':
     while True:
         try:
             # test json
+<<<<<<< HEAD
             # readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
 
             Proto_msg_to_ros.on_message_Flight_Information(readTenByte)
-
-        except KeyboardInterrupt as e:
+=======
+            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)
+>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
+
+        except Exception as e:
             print("End of program")
+<<<<<<< HEAD
             sys.exit()
+=======
+            print(readTenByte)
+            pass
+>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
 
 
 

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)

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,11 +2,11 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
-  ttyport: /dev/ttyUSB0
+  ttyport: /dev/ttyTHS1
 MQTT: "None"
 #ROS
 ROS:
   ROSClientNamePub: Drone550UAVLINKPub
   ROStopicName_Flight_Information: Flight_Information_reciver
 LOG:
-  logFileName: UAVLINKpub.log
+  logFileName: UAVLINKpub.log

class_model/src/utils/uavlinkConfig_SUB.yml

@@ -2,10 +2,14 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
+<<<<<<< HEAD
   ttyport: /dev/ttyUSB0
+=======
+  ttyport: /dev/ttyTHS1
+>>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
 MQTT: "None"
 ROS:
   ROSClientNameSub: Drone550UAVLINKSub
   Dron550_ROStopicName_Flight_Information: Flight_Information_reciver
 LOG:
-  logFileName: UAVLINKsub.log
+  logFileName: UAVLINKsub.log