From ac8c459d03d7b893c31949cf969ea8eae05c4362 Mon Sep 17 00:00:00 2001 From: Arthur Golubtsov Date: Wed, 13 Nov 2019 08:41:34 +0000 Subject: [PATCH] mavros_mavlink: Add load_param_file function --- Drone/mavros_mavlink.py | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/Drone/mavros_mavlink.py b/Drone/mavros_mavlink.py index feded7c..3b093da 100644 --- a/Drone/mavros_mavlink.py +++ b/Drone/mavros_mavlink.py @@ -1,12 +1,14 @@ import rospy import time +import logging from mavros_msgs.srv import CommandLong -from mavros_msgs.srv import ParamGet -from mavros_msgs.msg import State +from mavros_msgs.srv import ParamGet, ParamSet +from mavros_msgs.msg import State, ParamValue from pymavlink.dialects.v20 import common as mavlink send_command_long = rospy.ServiceProxy('/mavros/cmd/command', CommandLong) get_param = rospy.ServiceProxy('/mavros/param/get', ParamGet) +set_param = rospy.ServiceProxy('/mavros/param/set', ParamSet) system_status = -1 heartbeat_sub = None heartbeat_sub_status = None @@ -123,3 +125,25 @@ def stop_subscriber(): heartbeat_sub.unregister() heartbeat_sub_status = False +def load_param_file(px4_file): + result = True + try: + px4_params = open(px4_file) + except IOError: + logging.error("File {} can't be opened".format(filepath)) + result = False + else: + with open(px4_file) as px4_params: + for line in px4_params: + param_str_array = line[:-1].split('\t') + param_name = param_str_array[2] + param_value_str = param_str_array[3] + param_type = param_str_array[4] + if param_type == '6': + param_value = ParamValue(integer=int(param_value_str)) + else: + param_value = ParamValue(real=float(param_value_str)) + if not set_param(param_name, param_value): + result = False + return result +