forked from mavlink/mavlink
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge branch 'master' of https://github.com/tridge/pymavlink
- Loading branch information
Showing
17 changed files
with
1,244 additions
and
92 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,68 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| ''' | ||
| show GPS lock events in a MAVLink log | ||
| ''' | ||
|
|
||
| import sys, time, os | ||
|
|
||
| # allow import from the parent directory, where mavlink.py is | ||
| sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) | ||
|
|
||
| from optparse import OptionParser | ||
| parser = OptionParser("gpslock.py [options]") | ||
| parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") | ||
|
|
||
| (opts, args) = parser.parse_args() | ||
|
|
||
| if opts.mav10: | ||
| os.environ['MAVLINK10'] = '1' | ||
| import mavutil | ||
|
|
||
| if len(args) < 1: | ||
| print("Usage: gpslock.py [options] <LOGFILE...>") | ||
| sys.exit(1) | ||
|
|
||
| def lock_time(logfile): | ||
| '''work out gps lock times for a log file''' | ||
| print("Processing log %s" % filename) | ||
| mlog = mavutil.mavlink_connection(filename) | ||
|
|
||
| locked = False | ||
| start_time = 0.0 | ||
| total_time = 0.0 | ||
| t = None | ||
| m = mlog.recv_match(type='GPS_RAW') | ||
| unlock_time = time.mktime(time.localtime(m._timestamp)) | ||
|
|
||
| while True: | ||
| m = mlog.recv_match(type='GPS_RAW') | ||
| if m is None: | ||
| if locked: | ||
| total_time += time.mktime(t) - start_time | ||
| if total_time > 0: | ||
| print("Lock time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) | ||
| return total_time | ||
| t = time.localtime(m._timestamp) | ||
| if m.fix_type == 2 and not locked: | ||
| print("Locked at %s after %u seconds" % (time.asctime(t), | ||
| time.mktime(t) - unlock_time)) | ||
| locked = True | ||
| start_time = time.mktime(t) | ||
| elif m.fix_type == 1 and locked: | ||
| print("Lost GPS lock at %s" % time.asctime(t)) | ||
| locked = False | ||
| total_time += time.mktime(t) - start_time | ||
| unlock_time = time.mktime(t) | ||
| elif m.fix_type == 0 and locked: | ||
| print("Lost protocol lock at %s" % time.asctime(t)) | ||
| locked = False | ||
| total_time += time.mktime(t) - start_time | ||
| unlock_time = time.mktime(t) | ||
| return total_time | ||
|
|
||
| total = 0.0 | ||
| for filename in args: | ||
| total += lock_time(filename) | ||
|
|
||
| print("Total time locked: %u:%02u" % (int(total)/60, int(total)%60)) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,138 @@ | ||
| #!/usr/bin/env python | ||
|
|
||
| ''' | ||
| fit best estimate of magnetometer offsets | ||
| ''' | ||
|
|
||
| import sys, time, os, math | ||
|
|
||
| # allow import from the parent directory, where mavlink.py is | ||
| sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) | ||
|
|
||
| from optparse import OptionParser | ||
| parser = OptionParser("magfit.py [options]") | ||
| parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") | ||
| parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") | ||
| parser.add_option("--noise", type='float', default=0, help="noise to add") | ||
| parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") | ||
|
|
||
| (opts, args) = parser.parse_args() | ||
|
|
||
| if opts.mav10: | ||
| os.environ['MAVLINK10'] = '1' | ||
| import mavutil | ||
| from rotmat import Vector3 | ||
|
|
||
| if len(args) < 1: | ||
| print("Usage: magfit.py [options] <LOGFILE...>") | ||
| sys.exit(1) | ||
|
|
||
| def noise(): | ||
| '''a noise vector''' | ||
| from random import gauss | ||
| v = Vector3(gauss(0, 1), gauss(0, 1), gauss(0, 1)) | ||
| v.normalize() | ||
| return v * opts.noise | ||
|
|
||
| def select_data(data): | ||
| ret = [] | ||
| counts = {} | ||
| for d in data: | ||
| mag = d | ||
| key = "%u:%u:%u" % (mag.x/20,mag.y/20,mag.z/20) | ||
| if key in counts: | ||
| counts[key] += 1 | ||
| else: | ||
| counts[key] = 1 | ||
| if counts[key] < 3: | ||
| ret.append(d) | ||
| print(len(data), len(ret)) | ||
| return ret | ||
|
|
||
| def radius(mag, offsets): | ||
| '''return radius give data point and offsets''' | ||
| return (mag + offsets).length() | ||
|
|
||
| def radius_cmp(a, b, offsets): | ||
| '''return radius give data point and offsets''' | ||
| diff = radius(a, offsets) - radius(b, offsets) | ||
| if diff > 0: | ||
| return 1 | ||
| if diff < 0: | ||
| return -1 | ||
| return 0 | ||
|
|
||
| def sphere_error(p, data): | ||
| from scipy import sqrt | ||
| x,y,z,r = p | ||
| ofs = Vector3(x,y,z) | ||
| ret = [] | ||
| for d in data: | ||
| mag = d | ||
| err = r - radius(mag, ofs) | ||
| ret.append(err) | ||
| return ret | ||
|
|
||
| def fit_data(data): | ||
| import numpy, scipy | ||
| from scipy import optimize | ||
|
|
||
| p0 = [0.0, 0.0, 0.0, 0.0] | ||
| p1, ier = optimize.leastsq(sphere_error, p0[:], args=(data)) | ||
| if not ier in [1, 2, 3, 4]: | ||
| raise RuntimeError("Unable to find solution") | ||
| return (Vector3(p1[0], p1[1], p1[2]), p1[3]) | ||
|
|
||
| def magfit(logfile): | ||
| '''find best magnetometer offset fit to a log file''' | ||
|
|
||
| print("Processing log %s" % filename) | ||
| mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) | ||
|
|
||
| data = [] | ||
|
|
||
| last_t = 0 | ||
| offsets = Vector3(0,0,0) | ||
|
|
||
| # now gather all the data | ||
| while True: | ||
| m = mlog.recv_match(condition=opts.condition) | ||
| if m is None: | ||
| break | ||
| if m.get_type() == "SENSOR_OFFSETS": | ||
| # update current offsets | ||
| offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) | ||
| if m.get_type() == "RAW_IMU": | ||
| mag = Vector3(m.xmag, m.ymag, m.zmag) | ||
| # add data point after subtracting the current offsets | ||
| data.append(mag - offsets + noise()) | ||
|
|
||
| print("Extracted %u data points" % len(data)) | ||
| print("Current offsets: %s" % offsets) | ||
|
|
||
| data = select_data(data) | ||
|
|
||
| # do an initial fit with all data | ||
| (offsets, field_strength) = fit_data(data) | ||
|
|
||
| for count in range(3): | ||
| # sort the data by the radius | ||
| data.sort(lambda a,b : radius_cmp(a,b,offsets)) | ||
|
|
||
| print("Fit %u : %s field_strength=%6.1f to %6.1f" % ( | ||
| count, offsets, | ||
| radius(data[0], offsets), radius(data[-1], offsets))) | ||
|
|
||
| # discard outliers, keep the middle 3/4 | ||
| data = data[len(data)/8:-len(data)/8] | ||
|
|
||
| # fit again | ||
| (offsets, field_strength) = fit_data(data) | ||
|
|
||
| print("Final : %s field_strength=%6.1f to %6.1f" % ( | ||
| offsets, | ||
| radius(data[0], offsets), radius(data[-1], offsets))) | ||
|
|
||
| total = 0.0 | ||
| for filename in args: | ||
| magfit(filename) |
Oops, something went wrong.