mavparm.py
Go to the documentation of this file.
00001 '''
00002 module for loading/saving sets of mavlink parameters
00003 '''
00004 
00005 import fnmatch, math, time
00006 
00007 class MAVParmDict(dict):
00008     def __init__(self, *args):
00009         dict.__init__(self, args)
00010         # some parameters should not be loaded from files
00011         self.exclude_load = ['SYSID_SW_MREV', 'SYS_NUM_RESETS', 'ARSPD_OFFSET', 'GND_ABS_PRESS',
00012                              'GND_TEMP', 'CMD_TOTAL', 'CMD_INDEX', 'LOG_LASTFILE', 'FENCE_TOTAL',
00013                              'FORMAT_VERSION' ]
00014         self.mindelta = 0.000001
00015 
00016 
00017     def mavset(self, mav, name, value, retries=3):
00018         '''set a parameter on a mavlink connection'''
00019         got_ack = False
00020         while retries > 0 and not got_ack:
00021             retries -= 1
00022             mav.param_set_send(name.upper(), float(value))
00023             tstart = time.time()
00024             while time.time() - tstart < 1:
00025                 ack = mav.recv_match(type='PARAM_VALUE', blocking=False)
00026                 if ack == None:
00027                     time.sleep(0.1)
00028                     continue
00029                 if str(name).upper() == str(ack.param_id).upper():
00030                     got_ack = True
00031                     self.__setitem__(name, float(value))
00032                     break
00033         if not got_ack:
00034             print("timeout setting %s to %f" % (name, float(value)))
00035             return False
00036         return True
00037 
00038 
00039     def save(self, filename, wildcard='*', verbose=False):
00040         '''save parameters to a file'''
00041         f = open(filename, mode='w')
00042         k = list(self.keys())
00043         k.sort()
00044         count = 0
00045         for p in k:
00046             if p and fnmatch.fnmatch(str(p).upper(), wildcard.upper()):
00047                 f.write("%-16.16s %f\n" % (p, self.__getitem__(p)))
00048                 count += 1
00049         f.close()
00050         if verbose:
00051             print("Saved %u parameters to %s" % (count, filename))
00052 
00053 
00054     def load(self, filename, wildcard='*', mav=None, check=True):
00055         '''load parameters from a file'''
00056         try:
00057             f = open(filename, mode='r')
00058         except:
00059             print("Failed to open file '%s'" % filename)
00060             return False
00061         count = 0
00062         changed = 0
00063         for line in f:
00064             line = line.strip()
00065             if not line or line[0] == "#":
00066                 continue
00067             line = line.replace(',',' ')
00068             a = line.split()
00069             if len(a) != 2:
00070                 print("Invalid line: %s" % line)
00071                 continue
00072             # some parameters should not be loaded from files
00073             if a[0] in self.exclude_load:
00074                 continue
00075             if not fnmatch.fnmatch(a[0].upper(), wildcard.upper()):
00076                 continue
00077             if mav is not None:
00078                 if check:
00079                     if a[0] not in self.keys():
00080                         print("Unknown parameter %s" % a[0])
00081                         continue
00082                     old_value = self.__getitem__(a[0])
00083                     if math.fabs(old_value - float(a[1])) <= self.mindelta:
00084                         count += 1
00085                         continue
00086                     if self.mavset(mav, a[0], a[1]):
00087                         print("changed %s from %f to %f" % (a[0], old_value, float(a[1])))
00088                 else:
00089                     print("set %s to %f" % (a[0], float(a[1])))
00090                     self.mavset(mav, a[0], a[1])
00091                 changed += 1
00092             else:
00093                 self.__setitem__(a[0], float(a[1]))
00094             count += 1
00095         f.close()
00096         if mav is not None:
00097             print("Loaded %u parameters from %s (changed %u)" % (count, filename, changed))
00098         else:
00099             print("Loaded %u parameters from %s" % (count, filename))
00100         return True
00101 
00102     def show(self, wildcard='*'):
00103         '''show parameters'''
00104         k = sorted(self.keys())
00105         for p in k:
00106             if fnmatch.fnmatch(str(p).upper(), wildcard.upper()):
00107                 print("%-16.16s %f" % (str(p), self.get(p)))
00108 
00109     def diff(self, filename, wildcard='*'):
00110         '''show differences with another parameter file'''
00111         other = MAVParmDict()
00112         if not other.load(filename):
00113             return
00114         keys = sorted(list(set(self.keys()).union(set(other.keys()))))
00115         for k in keys:
00116             if not fnmatch.fnmatch(str(k).upper(), wildcard.upper()):
00117                 continue
00118             if not k in other:
00119                 print("%-16.16s              %12.4f" % (k, self[k]))
00120             elif not k in self:
00121                 print("%-16.16s %12.4f" % (k, other[k]))
00122             elif abs(self[k] - other[k]) > self.mindelta:
00123                 print("%-16.16s %12.4f %12.4f" % (k, other[k], self[k]))
00124                 
00125         


mavlink
Author(s): Lorenz Meier
autogenerated on Thu Jun 6 2019 19:01:57