mavparm.py
Go to the documentation of this file.
1 '''
2 module for loading/saving sets of mavlink parameters
3 '''
4 from __future__ import print_function
5 
6 
7 import fnmatch, math, time, struct
8 from pymavlink import mavutil
9 
10 class MAVParmDict(dict):
11  def __init__(self, *args):
12  dict.__init__(self, args)
13  # some parameters should not be loaded from files
14  self.exclude_load = ['SYSID_SW_MREV', 'SYS_NUM_RESETS', 'ARSPD_OFFSET', 'GND_ABS_PRESS',
15  'GND_TEMP', 'CMD_TOTAL', 'CMD_INDEX', 'LOG_LASTFILE', 'FENCE_TOTAL',
16  'FORMAT_VERSION' ]
17  self.mindelta = 0.000001
18 
19 
20  def mavset(self, mav, name, value, retries=3, parm_type=None):
21  '''set a parameter on a mavlink connection'''
22  got_ack = False
23 
24  if parm_type is not None and parm_type != mavutil.mavlink.MAV_PARAM_TYPE_REAL32:
25  # need to encode as a float for sending
26  if parm_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT8:
27  vstr = struct.pack(">xxxB", int(value))
28  elif parm_type == mavutil.mavlink.MAV_PARAM_TYPE_INT8:
29  vstr = struct.pack(">xxxb", int(value))
30  elif parm_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT16:
31  vstr = struct.pack(">xxH", int(value))
32  elif parm_type == mavutil.mavlink.MAV_PARAM_TYPE_INT16:
33  vstr = struct.pack(">xxh", int(value))
34  elif parm_type == mavutil.mavlink.MAV_PARAM_TYPE_UINT32:
35  vstr = struct.pack(">I", int(value))
36  elif parm_type == mavutil.mavlink.MAV_PARAM_TYPE_INT32:
37  vstr = struct.pack(">i", int(value))
38  else:
39  print("can't send %s of type %u" % (name, parm_type))
40  return False
41  vfloat, = struct.unpack(">f", vstr)
42  else:
43  vfloat = float(value)
44 
45  while retries > 0 and not got_ack:
46  retries -= 1
47  mav.param_set_send(name.upper(), vfloat, parm_type=parm_type)
48  tstart = time.time()
49  while time.time() - tstart < 1:
50  ack = mav.recv_match(type='PARAM_VALUE', blocking=False)
51  if ack is None:
52  time.sleep(0.1)
53  continue
54  if str(name).upper() == str(ack.param_id).upper():
55  got_ack = True
56  self.__setitem__(name, float(value))
57  break
58  if not got_ack:
59  print("timeout setting %s to %f" % (name, vfloat))
60  return False
61  return True
62 
63 
64  def save(self, filename, wildcard='*', verbose=False):
65  '''save parameters to a file'''
66  f = open(filename, mode='w')
67  k = list(self.keys())
68  k.sort()
69  count = 0
70  for p in k:
71  if p and fnmatch.fnmatch(str(p).upper(), wildcard.upper()):
72  value = self.__getitem__(p)
73  if isinstance(value, float):
74  f.write("%-16.16s %f\n" % (p, value))
75  else:
76  f.write("%-16.16s %s\n" % (p, str(value)))
77  count += 1
78  f.close()
79  if verbose:
80  print("Saved %u parameters to %s" % (count, filename))
81 
82 
83  def load(self, filename, wildcard='*', mav=None, check=True, use_excludes=True):
84  '''load parameters from a file'''
85  try:
86  f = open(filename, mode='r')
87  except:
88  print("Failed to open file '%s'" % filename)
89  return False
90  count = 0
91  changed = 0
92  for line in f:
93  line = line.strip()
94  if not line or line[0] == "#":
95  continue
96  line = line.replace(',',' ')
97  a = line.split()
98  if len(a) != 2:
99  print("Invalid line: %s" % line)
100  continue
101  # some parameters should not be loaded from files
102  if use_excludes and a[0] in self.exclude_load:
103  continue
104  if not fnmatch.fnmatch(a[0].upper(), wildcard.upper()):
105  continue
106  if mav is not None:
107  if check:
108  if a[0] not in list(self.keys()):
109  print("Unknown parameter %s" % a[0])
110  continue
111  old_value = self.__getitem__(a[0])
112  if math.fabs(old_value - float(a[1])) <= self.mindelta:
113  count += 1
114  continue
115  if self.mavset(mav, a[0], a[1]):
116  print("changed %s from %f to %f" % (a[0], old_value, float(a[1])))
117  else:
118  print("set %s to %f" % (a[0], float(a[1])))
119  self.mavset(mav, a[0], a[1])
120  changed += 1
121  else:
122  self.__setitem__(a[0], float(a[1]))
123  count += 1
124  f.close()
125  if mav is not None:
126  print("Loaded %u parameters from %s (changed %u)" % (count, filename, changed))
127  else:
128  print("Loaded %u parameters from %s" % (count, filename))
129  return True
130 
131  def show_param_value(self, name, value):
132  print("%-16.16s %s" % (name, value))
133 
134  def show(self, wildcard='*'):
135  '''show parameters'''
136  k = sorted(self.keys())
137  for p in k:
138  if fnmatch.fnmatch(str(p).upper(), wildcard.upper()):
139  self.show_param_value(str(p), "%f" % self.get(p))
140 
141  def diff(self, filename, wildcard='*', use_excludes=True):
142  '''show differences with another parameter file'''
143  other = MAVParmDict()
144  if not other.load(filename, use_excludes=use_excludes):
145  return
146  keys = sorted(list(set(self.keys()).union(set(other.keys()))))
147  for k in keys:
148  if not fnmatch.fnmatch(str(k).upper(), wildcard.upper()):
149  continue
150  if not k in other:
151  value = float(self[k])
152  print("%-16.16s %12.4f" % (k, value))
153  elif not k in self:
154  print("%-16.16s %12.4f" % (k, float(other[k])))
155  elif abs(self[k] - other[k]) > self.mindelta:
156  value = float(self[k])
157  print("%-16.16s %12.4f %12.4f" % (k, other[k], value))
158 
159 


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Apr 7 2019 02:06:02