magfit_motors.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 '''
00004 fit best estimate of magnetometer offsets, trying to take into account motor interference
00005 '''
00006 
00007 import sys, time, os, math
00008 
00009 from argparse import ArgumentParser
00010 parser = ArgumentParser(description=__doc__)
00011 parser.add_argument("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps")
00012 parser.add_argument("--condition",dest="condition", default=None, help="select packets by condition")
00013 parser.add_argument("--noise", type=float, default=0, help="noise to add")
00014 parser.add_argument("logs", metavar="LOG", nargs="+")
00015 
00016 args = parser.parse_args()
00017 
00018 from pymavlink import mavutil
00019 from pymavlink.rotmat import Vector3
00020 
00021 
00022 def noise():
00023     '''a noise vector'''
00024     from random import gauss
00025     v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1))
00026     v.normalize()
00027     return v * args.noise
00028 
00029 def select_data(data):
00030     ret = []
00031     counts = {}
00032     for d in data:
00033         (mag,motor) = d
00034         key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20)
00035         if key in counts:
00036             counts[key] += 1
00037         else:
00038             counts[key] = 1
00039         if counts[key] < 3:
00040             ret.append(d)
00041     print(len(data), len(ret))
00042     return ret
00043 
00044 def radius(d, offsets, motor_ofs):
00045     '''return radius give data point and offsets'''
00046     (mag, motor) = d
00047     return (mag + offsets + motor*motor_ofs).length()
00048 
00049 def radius_cmp(a, b, offsets, motor_ofs):
00050     '''return radius give data point and offsets'''
00051     diff = radius(a, offsets, motor_ofs) - radius(b, offsets, motor_ofs)
00052     if diff > 0:
00053         return 1
00054     if diff < 0:
00055         return -1
00056     return 0
00057 
00058 def sphere_error(p, data):
00059     from scipy import sqrt
00060     x,y,z,mx,my,mz,r = p
00061     ofs = Vector3(x,y,z)
00062     motor_ofs = Vector3(mx,my,mz)
00063     ret = []
00064     for d in data:
00065         (mag,motor) = d
00066         err = r - radius((mag,motor), ofs, motor_ofs)
00067         ret.append(err)
00068     return ret
00069 
00070 def fit_data(data):
00071     import numpy, scipy
00072     from scipy import optimize
00073 
00074     p0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
00075     p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data))
00076     if not ier in [1, 2, 3, 4]:
00077         raise RuntimeError("Unable to find solution")
00078     return (Vector3(p1[0], p1[1], p1[2]), Vector3(p1[3], p1[4], p1[5]), p1[6])
00079 
00080 def magfit(logfile):
00081     '''find best magnetometer offset fit to a log file'''
00082 
00083     print("Processing log %s" % filename)
00084     mlog = mavutil.mavlink_connection(filename, notimestamps=args.notimestamps)
00085 
00086     data = []
00087 
00088     last_t = 0
00089     offsets = Vector3(0,0,0)
00090     motor_ofs = Vector3(0,0,0)
00091     motor = 0.0
00092 
00093     # now gather all the data
00094     while True:
00095         m = mlog.recv_match(condition=args.condition)
00096         if m is None:
00097             break
00098         if m.get_type() == "PARAM_VALUE" and m.param_id == "RC3_MIN":
00099             rc3_min = float(m.param_value)
00100         if m.get_type() == "SENSOR_OFFSETS":
00101             # update current offsets
00102             offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z)
00103         if m.get_type() == "SERVO_OUTPUT_RAW":
00104             motor_pwm = m.servo1_raw + m.servo2_raw + m.servo3_raw + m.servo4_raw
00105             motor_pwm *= 0.25
00106             rc3_min = mlog.param('RC3_MIN', 1100)
00107             rc3_max = mlog.param('RC3_MAX', 1900)
00108             motor = (motor_pwm - rc3_min) / (rc3_max - rc3_min)
00109             if motor > 1.0:
00110                 motor = 1.0
00111             if motor < 0.0:
00112                 motor = 0.0
00113         if m.get_type() == "RAW_IMU":
00114             mag = Vector3(m.xmag, m.ymag, m.zmag)
00115             # add data point after subtracting the current offsets
00116             data.append((mag - offsets + noise(), motor))
00117 
00118     print("Extracted %u data points" % len(data))
00119     print("Current offsets: %s" % offsets)
00120 
00121     data = select_data(data)
00122 
00123     # do an initial fit with all data
00124     (offsets, motor_ofs, field_strength) = fit_data(data)
00125 
00126     for count in range(3):
00127         # sort the data by the radius
00128         data.sort(lambda a,b : radius_cmp(a,b,offsets,motor_ofs))
00129 
00130         print("Fit %u    : %s  %s field_strength=%6.1f to %6.1f" % (
00131             count, offsets, motor_ofs,
00132             radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs)))
00133 
00134         # discard outliers, keep the middle 3/4
00135         data = data[len(data)/8:-len(data)/8]
00136 
00137         # fit again
00138         (offsets, motor_ofs, field_strength) = fit_data(data)
00139 
00140     print("Final    : %s  %s field_strength=%6.1f to %6.1f" % (
00141         offsets, motor_ofs,
00142         radius(data[0], offsets, motor_ofs), radius(data[-1], offsets, motor_ofs)))
00143     print("mavgraph.py '%s' 'mag_field(RAW_IMU)' 'mag_field_motors(RAW_IMU,SENSOR_OFFSETS,(%f,%f,%f),SERVO_OUTPUT_RAW,(%f,%f,%f))'" % (
00144         filename,
00145         offsets.x,offsets.y,offsets.z,
00146         motor_ofs.x, motor_ofs.y, motor_ofs.z))
00147 
00148 total = 0.0
00149 for filename in args.logs:
00150     magfit(filename)


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43