uavlink update

class_model/src/uavlinkPubMain.py

@@ -21,52 +21,49 @@ from geometry_msgs.msg import Vector3
 
 
 def init_dataFormat(cfg:Read_PUB_Config):
-    f1data = b'\xf1drone550pub...........\r\n'
+    Proto_msg_from_ros.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.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()
-
+    Proto_msg_from_ros.rate = rospy.Rate(10)
     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")
     cfg = Read_PUB_Config(FilePath)
+    
 
     # set log
-    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
-    formatter = logging.Formatter(log_format)
+    file_log_format = "%(message)s"
+    file_formatter = logging.Formatter(file_log_format)
+    stream_formatter = logging.Formatter(stream_log_format)
 
     stream_handler = logging.StreamHandler()
-    stream_handler.setFormatter(formatter)
+    stream_handler.setFormatter(stream_formatter)
     stream_handler.setLevel(logging.DEBUG)
 
-    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler = logging.FileHandler(cfg.logFileName, mode='w')
-    file_handler.setFormatter(formatter)
+    file_handler.setFormatter(file_formatter)
     file_handler.setLevel(logging.INFO)
 
     logger = logging.getLogger("__UAVLINKSUBPUB__")
-    logger.setLevel(logging.INFO)
+    logger.setLevel(logging.DEBUG)
     logger.addHandler(file_handler)
     logger.addHandler(stream_handler)
-    logger.info(cfg)    
+    logger.debug(cfg)
-
-
 
     # Ros
     rosClient = cfg.ROSClientNamePub
     rospy.init_node(rosClient)
-    
+
     # init data format
     init_dataFormat(cfg)
 
     try:
         rospy.spin()
-    except BaseException as error:
+    except KeyboardInterrupt as e:
-        logger.info("End of program")
+        Proto_msg_from_ros.turnOffUavlink()
+        print("End of program")
         sys.exit()
 
      

class_model/src/uavlinkSubMain.py

@@ -8,6 +8,7 @@ import proto.flight_information_pb2 as flight_information_pb2
 import logging
 from utils.readConfig import Read_SUB_Config
 from utils.proto_uavlink_pub_data_to_ros import Proto_msg_to_ros
+import google.protobuf.message
 
 import rospy
 from std_msgs.msg import String
@@ -18,95 +19,59 @@ from sensor_msgs.msg import NavSatFix
 from sensor_msgs.msg import Imu
 from sensor_msgs.msg import Range
 from geometry_msgs.msg import Vector3
+# costom msg
+from mqtt.msg import FlightInformation
 
 
 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)
+    Proto_msg_to_ros.publisher_Flight_Information = rospy.Publisher(cfg.topicName_Flight_Information,FlightInformation,queue_size=10)
-
+    Proto_msg_to_ros.FlightInformationRosMsg = FlightInformation()
     
 if __name__ == '__main__':
     FilePath = os.path.join(os.path.dirname(__file__),"utils","uavlinkConfig_SUB.yml")
     cfg = Read_SUB_Config(FilePath)
     
-    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE, timeout = 0.5)
+    sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
     # set log
-    log_format = "%(asctime)s - %(levelname)s - %(message)s"
+    stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
-    formatter = logging.Formatter(log_format)
+    file_log_format = "%(message)s"
+    file_formatter = logging.Formatter(file_log_format)
+    stream_formatter = logging.Formatter(stream_log_format)
 
     stream_handler = logging.StreamHandler()
-    stream_handler.setFormatter(formatter)
+    stream_handler.setFormatter(stream_formatter)
     stream_handler.setLevel(logging.DEBUG)
 
-    file_handler = logging.FileHandler(cfg.logFileName)
+    file_handler = logging.FileHandler(cfg.logFileName, mode='w')
-    file_handler.setFormatter(formatter)
+    file_handler.setFormatter(file_formatter)
     file_handler.setLevel(logging.INFO)
 
     logger = logging.getLogger("__UAVLINKSUB__")
-    logger.setLevel(logging.INFO)
+    logger.setLevel(logging.DEBUG)
     logger.addHandler(file_handler)
     logger.addHandler(stream_handler)
-    logger.info(cfg)
+    logger.debug(cfg)
-    
 
     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
-    flight_information_msg.gps.LON = 23423.1231515
-    flight_information_msg.gps.ALT = 12123.1231515
-    flight_information_msg.heading = 155.12215
-    proto = flight_information_msg.SerializeToString()
-    readTenByte = b'\xf1' + proto + b'\r\x1a'
     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)
-=======
             if sel.in_waiting >= 25:
-                readTenByte = sel.read_until(expected= b'\x01\x01', size=25)
+                readTenByte = sel.read_until(expected= b'\x01\x17', 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:
+        except google.protobuf.message.DecodeError as e:
+            logger.info("DecodeError:{}".format(e))
+            logger.info("readTenByte:{}".format(readTenByte))
+        except KeyboardInterrupt as e:
+            Proto_msg_to_ros.turnOffUavlink()
             print("End of program")
-<<<<<<< HEAD
             sys.exit()
-=======
-            print(readTenByte)
-            pass
->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
 
 
 

class_model/src/utils/proto_uavlink_pub_data_to_ros.py

@@ -3,7 +3,8 @@ import time
 import proto.flight_information_pb2 as flight_information_pb2
 import google.protobuf.json_format as json_format
 import logging
-
+# costom msg
+from mqtt.msg import FlightInformation
 # TODO: use native ros type instead of json or str
 logger = logging.getLogger("__SUB__")
 
@@ -12,20 +13,39 @@ class Proto_msg_to_ros:
     flight_information_msg = None
 
     #Ros publisher
+    FlightInformationRosMsg = None
     rate = None
     publisher_Flight_Information = None
 
+    #uavlink 
+    sel = None
+    payload = b"......................"
+    noEcho_code = b"\x0d\x0a"
+    echo_code = b"\x0d\x1a"
+    close_code = b"\x0d\x2a"
+    f1_code = b"\xf1"
+    f2_code = b"\xf2"
+    f1_close_code = f1_code + payload + close_code
+    f2_close_code = f2_code + payload + close_code
 
     #Proto
     @classmethod
     def on_message_Flight_Information(cls, msg):
-        proto = msg[1:-2]
+        cls.publisher_Flight_Information.publish(cls.convert_proto_to_ros(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)
-        print(protoTOJson_msg)
-        cls.publisher_Flight_Information.publish(protoTOJson_msg)
         cls.rate.sleep()
+    
+    @classmethod
+    def convert_proto_to_ros(cls, proto):
+        proto_msg = cls.flight_information_msg.FromString(proto)
+        cls.FlightInformationRosMsg.LAT = proto_msg.gps.LAT
+        cls.FlightInformationRosMsg.LON = proto_msg.gps.LON
+        cls.FlightInformationRosMsg.ALT = proto_msg.gps.ALT
+        cls.FlightInformationRosMsg.heading = proto_msg.heading
+        return cls.FlightInformationRosMsg
 
-        
+    @classmethod
+    def turnOffUavlink(cls):
+        time.sleep(0.5)
+        cls.sel.write(cls.f1_close_code)
+        time.sleep(0.5)
+        cls.sel.write(cls.f2_close_code)    

class_model/src/utils/proto_uavlink_sub_data_from_ros.py

@@ -2,12 +2,23 @@ import orjson
 import time
 import logging
 
-# logger = logging.getLogger("__PUB__")
+logger = logging.getLogger("__UAVLINKSUBPUB__")
 
 class Proto_msg_from_ros:
     #Protobuf
     flight_information_msg = None
+    #ros
+    rate = None
+    #uavlink 
     sel = None
+    payload = b"......................"
+    noEcho_code = b"\x0d\x0a"
+    echo_code = b"\x0d\x1a"
+    close_code = b"\x0d\x2a"
+    f1_code = b"\xf1"
+    f2_code = b"\xf2"
+    f1_close_code = f1_code + payload + close_code
+    f2_close_code = f2_code + payload + close_code
 
 
     @classmethod
@@ -15,14 +26,19 @@ class Proto_msg_from_ros:
         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')
+        cls.sel.write(cls.f2_code + flightInformationMsg + cls.noEcho_code)
-        print(len(b'\xf2' + flightInformationMsg + b'\r\n'))
+        cls.rate.sleep()
-        readTenByte = cls.sel.readline()
+
-        print(readTenByte)
+
+    @classmethod
+    def turnOffUavlink(cls):
+        time.sleep(0.5)
+        cls.sel.write(cls.f1_close_code)
+        time.sleep(0.5)
+        cls.sel.write(cls.f2_close_code)

class_model/src/utils/uavlinkConfig_PUB.yml

@@ -1,6 +1,6 @@
 UAVLINK:
   uavlink_msg_format: Proto
-  uav_id: \x01\x01
+  uav_id: \x01\x17
   baudrate: 250000
   ttyport: /dev/ttyTHS1
 MQTT: "None"

class_model/src/utils/uavlinkConfig_SUB.yml

@@ -2,11 +2,7 @@ UAVLINK:
   uavlink_msg_format: Proto
   uav_id: \x01\x01
   baudrate: 250000
-<<<<<<< HEAD
-  ttyport: /dev/ttyUSB0
-=======
   ttyport: /dev/ttyTHS1
->>>>>>> 8c914b819bf1e908a923a85d37f0af02a44da7d5
 MQTT: "None"
 ROS:
   ROSClientNameSub: Drone550UAVLINKSub