Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

uavlink update

Browse Source
This commit is contained in:
RangeOfGlitching
2023-04-15 15:31:53 +08:00
parent 3597e3d901
commit b09489cc80
6 changed files with 91 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/src/uavlinkPubMain.py
+15 -18
View File
@@ -21,14 +21,9 @@ from geometry_msgs.msg import Vector3
def init_dataFormat(cfg:Read_PUB_Config):
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.sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
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)
@@ -36,25 +31,26 @@ 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"
formatter = logging.Formatter(log_format)
stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
file_handler = logging.FileHandler(cfg.logFileName, mode='w')
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
@@ -65,8 +61,9 @@ if __name__ == '__main__':
try:
rospy.spin()
except BaseException as error:
logger.info("End of program")
except KeyboardInterrupt as e:
Proto_msg_from_ros.turnOffUavlink()
print("End of program")
sys.exit()
class_model/src/uavlinkSubMain.py
+22 -57
View File
@@ -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"
formatter = logging.Formatter(log_format)
stream_log_format = "%(asctime)s - %(levelname)s - %(message)s"
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.setFormatter(formatter)
file_handler = logging.FileHandler(cfg.logFileName, mode='w')
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)
# print(readTenByte)
if b"drone550" in readTenByte:
print(readTenByte)
print("radar")
continue
readTenByte = sel.read_until(expected= b'\x01\x17', size=25)
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
+28 -8
View File
@@ -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]
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.publisher_Flight_Information.publish(cls.convert_proto_to_ros(msg[1:-2]))
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
+23 -7
View File
@@ -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')
print(len(b'\xf2' + flightInformationMsg + b'\r\n'))
readTenByte = cls.sel.readline()
print(readTenByte)
cls.sel.write(cls.f2_code + flightInformationMsg + cls.noEcho_code)
cls.rate.sleep()
@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 -1
View File
@@ -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
-4
View File
@@ -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