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
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
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