diff options
Diffstat (limited to 'mavlink/share/pyshared/pymavlink/examples')
18 files changed, 0 insertions, 1739 deletions
diff --git a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py b/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py deleted file mode 100644 index d7d82256c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -''' -set stream rate on an APM -''' - -import sys, struct, 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("apmsetrate.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -parser.add_option("--rate", dest="rate", default=4, type='int', help="requested stream rate") -parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', - default=255, help='MAVLink source system for this GCS') -parser.add_option("--showmessages", dest="showmessages", action='store_true', - help="show incoming messages", default=False) -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 mavlink10 as mavlink -else: - import mavlink -import mavutil - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -def wait_heartbeat(m): - '''wait for a heartbeat so we know the target system IDs''' - print("Waiting for APM heartbeat") - m.wait_heartbeat() - print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) - -def show_messages(m): - '''show incoming mavlink messages''' - while True: - msg = m.recv_match(blocking=True) - if not msg: - return - if msg.get_type() == "BAD_DATA": - if mavutil.all_printable(msg.data): - sys.stdout.write(msg.data) - sys.stdout.flush() - else: - print(msg) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) - -# wait for the heartbeat msg to find the system ID -wait_heartbeat(master) - -print("Sending all stream request for rate %u" % opts.rate) -for i in range(0, 3): - master.mav.request_data_stream_send(master.target_system, master.target_component, - mavlink.MAV_DATA_STREAM_ALL, opts.rate, 1) -if opts.showmessages: - show_messages(master) diff --git a/mavlink/share/pyshared/pymavlink/examples/bwtest.py b/mavlink/share/pyshared/pymavlink/examples/bwtest.py deleted file mode 100644 index de56d4f8c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/bwtest.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python - -''' -check bandwidth of link -''' - -import sys, struct, 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__)), '..')) - -import mavutil - -from optparse import OptionParser -parser = OptionParser("bwtest.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -(opts, args) = parser.parse_args() - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate) - -t1 = time.time() - -counts = {} - -bytes_sent = 0 -bytes_recv = 0 - -while True: - master.mav.heartbeat_send(1, 1) - master.mav.sys_status_send(1, 2, 3, 4, 5, 6, 7) - master.mav.gps_raw_send(1, 2, 3, 4, 5, 6, 7, 8, 9) - master.mav.attitude_send(1, 2, 3, 4, 5, 6, 7) - master.mav.vfr_hud_send(1, 2, 3, 4, 5, 6) - while master.port.inWaiting() > 0: - m = master.recv_msg() - if m == None: break - if m.get_type() not in counts: - counts[m.get_type()] = 0 - counts[m.get_type()] += 1 - t2 = time.time() - if t2 - t1 > 1.0: - print("%u sent, %u received, %u errors bwin=%.1f kB/s bwout=%.1f kB/s" % ( - master.mav.total_packets_sent, - master.mav.total_packets_received, - master.mav.total_receive_errors, - 0.001*(master.mav.total_bytes_received-bytes_recv)/(t2-t1), - 0.001*(master.mav.total_bytes_sent-bytes_sent)/(t2-t1))) - bytes_sent = master.mav.total_bytes_sent - bytes_recv = master.mav.total_bytes_received - t1 = t2 diff --git a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py b/mavlink/share/pyshared/pymavlink/examples/flightmodes.py deleted file mode 100644 index 03d7e2c47..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/flightmodes.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python - -''' -show changes in flight modes -''' - -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("flightmodes.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: flightmodes.py [options] <LOGFILE...>") - sys.exit(1) - -def flight_modes(logfile): - '''show flight modes for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename) - - mode = -1 - nav_mode = -1 - - filesize = os.path.getsize(filename) - - while True: - m = mlog.recv_match(type='SYS_STATUS', - condition='SYS_STATUS.mode != %u or SYS_STATUS.nav_mode != %u' % (mode, nav_mode)) - if m is None: - return - mode = m.mode - nav_mode = m.nav_mode - pct = (100.0 * mlog.f.tell()) / filesize - print('%s MAV.flightmode=%-12s mode=%u nav_mode=%u (MAV.timestamp=%u %u%%)' % ( - time.asctime(time.localtime(m._timestamp)), - mlog.flightmode, - mode, nav_mode, m._timestamp, pct)) - -for filename in args: - flight_modes(filename) - - diff --git a/mavlink/share/pyshared/pymavlink/examples/flighttime.py b/mavlink/share/pyshared/pymavlink/examples/flighttime.py deleted file mode 100644 index 81cd38c94..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/flighttime.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -''' -work out total flight time for 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("flighttime.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: flighttime.py [options] <LOGFILE...>") - sys.exit(1) - -def flight_time(logfile): - '''work out flight time for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename) - - in_air = False - start_time = 0.0 - total_time = 0.0 - t = None - - while True: - m = mlog.recv_match(type='VFR_HUD') - if m is None: - if in_air: - total_time += time.mktime(t) - start_time - if total_time > 0: - print("Flight time : %u:%02u" % (int(total_time)/60, int(total_time)%60)) - return total_time - t = time.localtime(m._timestamp) - if m.groundspeed > 3.0 and not in_air: - print("In air at %s" % time.asctime(t)) - in_air = True - start_time = time.mktime(t) - elif m.groundspeed < 3.0 and in_air: - print("On ground at %s" % time.asctime(t)) - in_air = False - total_time += time.mktime(t) - start_time - return total_time - -total = 0.0 -for filename in args: - total += flight_time(filename) - -print("Total time in air: %u:%02u" % (int(total)/60, int(total)%60)) diff --git a/mavlink/share/pyshared/pymavlink/examples/gpslock.py b/mavlink/share/pyshared/pymavlink/examples/gpslock.py deleted file mode 100644 index f15b29072..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/gpslock.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/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)) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit.py b/mavlink/share/pyshared/pymavlink/examples/magfit.py deleted file mode 100644 index 7bfda796b..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit.py +++ /dev/null @@ -1,138 +0,0 @@ -#!/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) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py b/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py deleted file mode 100644 index 87d2dbb7f..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python - -''' -fit best estimate of magnetometer offsets using the algorithm from -Bill Premerlani -''' - -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__)), '..')) - -# command line option handling -from optparse import OptionParser -parser = OptionParser("magfit_delta.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("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--verbose", action='store_true', default=False, help="verbose offset output") -parser.add_option("--gain", type='float', default=0.01, help="algorithm gain") -parser.add_option("--noise", type='float', default=0, help="noise to add") -parser.add_option("--max-change", type='float', default=10, help="max step change") -parser.add_option("--min-diff", type='float', default=50, help="min mag vector delta") -parser.add_option("--history", type='int', default=20, help="how many points to keep") -parser.add_option("--repeat", type='int', default=1, help="number of repeats through the data") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil -from rotmat import Vector3, Matrix3 - -if len(args) < 1: - print("Usage: magfit_delta.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 find_offsets(data, ofs): - '''find mag offsets by applying Bills "offsets revisited" algorithm - on the data - - This is an implementation of the algorithm from: - http://gentlenav.googlecode.com/files/MagnetometerOffsetNullingRevisited.pdf - ''' - - # a limit on the maximum change in each step - max_change = opts.max_change - - # the gain factor for the algorithm - gain = opts.gain - - data2 = [] - for d in data: - d = d.copy() + noise() - d.x = float(int(d.x + 0.5)) - d.y = float(int(d.y + 0.5)) - d.z = float(int(d.z + 0.5)) - data2.append(d) - data = data2 - - history_idx = 0 - mag_history = data[0:opts.history] - - for i in range(opts.history, len(data)): - B1 = mag_history[history_idx] + ofs - B2 = data[i] + ofs - - diff = B2 - B1 - diff_length = diff.length() - if diff_length <= opts.min_diff: - # the mag vector hasn't changed enough - we don't get any - # information from this - history_idx = (history_idx+1) % opts.history - continue - - mag_history[history_idx] = data[i] - history_idx = (history_idx+1) % opts.history - - # equation 6 of Bills paper - delta = diff * (gain * (B2.length() - B1.length()) / diff_length) - - # limit the change from any one reading. This is to prevent - # single crazy readings from throwing off the offsets for a long - # time - delta_length = delta.length() - if max_change != 0 and delta_length > max_change: - delta *= max_change / delta_length - - # set the new offsets - ofs = ofs - delta - - if opts.verbose: - print ofs - return ofs - - -def magfit(logfile): - '''find best magnetometer offset fit to a log file''' - - print("Processing log %s" % filename) - - # open the log file - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - - data = [] - mag = None - offsets = Vector3(0,0,0) - - # now gather all the data - while True: - # get the next MAVLink message in the log - m = mlog.recv_match(condition=opts.condition) - if m is None: - break - if m.get_type() == "SENSOR_OFFSETS": - # update offsets that were used during this flight - offsets = Vector3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - if m.get_type() == "RAW_IMU" and offsets != None: - # extract one mag vector, removing the offsets that were - # used during that flight to get the raw sensor values - mag = Vector3(m.xmag, m.ymag, m.zmag) - offsets - data.append(mag) - - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % offsets) - - # run the fitting algorithm - ofs = offsets - ofs = Vector3(0,0,0) - for r in range(opts.repeat): - ofs = find_offsets(data, ofs) - print('Loop %u offsets %s' % (r, ofs)) - sys.stdout.flush() - print("New offsets: %s" % ofs) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py b/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py deleted file mode 100644 index 30ba45806..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py +++ /dev/null @@ -1,159 +0,0 @@ -#!/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("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--minspeed", type='float', default=5.0, help="minimum ground speed to use") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: magfit.py [options] <LOGFILE...>") - sys.exit(1) - -class vec3(object): - def __init__(self, x, y, z): - self.x = x - self.y = y - self.z = z - def __str__(self): - return "%.1f %.1f %.1f" % (self.x, self.y, self.z) - -def heading_error1(parm, data): - from math import sin, cos, atan2, degrees - from numpy import dot - xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm - - ret = [] - for d in data: - x = d[0] + xofs - y = d[1] + yofs - z = d[2] + zofs - r = d[3] - p = d[4] - h = d[5] - - headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) - headY = y*cos(r) - z*sin(r) - heading = degrees(atan2(-headY,headX)) + declination - if heading < 0: - heading += 360 - herror = h - heading - if herror > 180: - herror -= 360 - if herror < -180: - herror += 360 - ret.append(herror) - return ret - -def heading_error(parm, data): - from math import sin, cos, atan2, degrees - from numpy import dot - xofs,yofs,zofs,a1,a2,a3,a4,a5,a6,a7,a8,a9,declination = parm - - a = [[1.0,a2,a3],[a4,a5,a6],[a7,a8,a9]] - - ret = [] - for d in data: - x = d[0] + xofs - y = d[1] + yofs - z = d[2] + zofs - r = d[3] - p = d[4] - h = d[5] - mv = [x, y, z] - mv2 = dot(a, mv) - x = mv2[0] - y = mv2[1] - z = mv2[2] - - headX = x*cos(p) + y*sin(r)*sin(p) + z*cos(r)*sin(p) - headY = y*cos(r) - z*sin(r) - heading = degrees(atan2(-headY,headX)) + declination - if heading < 0: - heading += 360 - herror = h - heading - if herror > 180: - herror -= 360 - if herror < -180: - herror += 360 - ret.append(herror) - return ret - -def fit_data(data): - import numpy, scipy - from scipy import optimize - - p0 = [0.0, 0.0, 0.0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0] - p1, ier = optimize.leastsq(heading_error1, p0[:], args=(data)) - -# p0 = p1[:] -# p1, ier = optimize.leastsq(heading_error, p0[:], args=(data)) - - print(p1) - if not ier in [1, 2, 3, 4]: - raise RuntimeError("Unable to find solution") - return p1 - -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) - - flying = False - gps_heading = 0.0 - - data = [] - - # get the current mag offsets - m = mlog.recv_match(type='SENSOR_OFFSETS') - offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - - attitude = mlog.recv_match(type='ATTITUDE') - - # now gather all the data - while True: - m = mlog.recv_match() - if m is None: - break - if m.get_type() == "GPS_RAW": - # flying if groundspeed more than 5 m/s - flying = (m.v > opts.minspeed and m.fix_type == 2) - gps_heading = m.hdg - if m.get_type() == "ATTITUDE": - attitude = m - if m.get_type() == "SENSOR_OFFSETS": - # update current offsets - offsets = vec3(m.mag_ofs_x, m.mag_ofs_y, m.mag_ofs_z) - if not flying: - continue - if m.get_type() == "RAW_IMU": - data.append((m.xmag - offsets.x, m.ymag - offsets.y, m.zmag - offsets.z, attitude.roll, attitude.pitch, gps_heading)) - print("Extracted %u data points" % len(data)) - print("Current offsets: %s" % offsets) - ofs2 = fit_data(data) - print("Declination estimate: %.1f" % ofs2[-1]) - new_offsets = vec3(ofs2[0], ofs2[1], ofs2[2]) - a = [[ofs2[3], ofs2[4], ofs2[5]], - [ofs2[6], ofs2[7], ofs2[8]], - [ofs2[9], ofs2[10], ofs2[11]]] - print(a) - print("New offsets : %s" % new_offsets) - -total = 0.0 -for filename in args: - magfit(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/magtest.py b/mavlink/share/pyshared/pymavlink/examples/magtest.py deleted file mode 100644 index 8b910f8fd..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/magtest.py +++ /dev/null @@ -1,120 +0,0 @@ -#!/usr/bin/env python - -''' -rotate APMs on bench to test magnetometers - -''' - -import sys, os, time -from math import radians - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavlink, mavutil - -from optparse import OptionParser -parser = OptionParser("rotate.py [options]") - -parser.add_option("--device1", dest="device1", default=None, help="mavlink device1") -parser.add_option("--device2", dest="device2", default=None, help="mavlink device2") -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -(opts, args) = parser.parse_args() - -if opts.device1 is None or opts.device2 is None: - print("You must specify a mavlink device") - sys.exit(1) - -def set_attitude(rc3, rc4): - global mav1, mav2 - values = [ 65535 ] * 8 - values[2] = rc3 - values[3] = rc4 - mav1.mav.rc_channels_override_send(mav1.target_system, mav1.target_component, *values) - mav2.mav.rc_channels_override_send(mav2.target_system, mav2.target_component, *values) - - -# create a mavlink instance -mav1 = mavutil.mavlink_connection(opts.device1, baud=opts.baudrate) - -# create a mavlink instance -mav2 = mavutil.mavlink_connection(opts.device2, baud=opts.baudrate) - -print("Waiting for HEARTBEAT") -mav1.wait_heartbeat() -mav2.wait_heartbeat() -print("Heartbeat from APM (system %u component %u)" % (mav1.target_system, mav1.target_system)) -print("Heartbeat from APM (system %u component %u)" % (mav2.target_system, mav2.target_system)) - -print("Waiting for MANUAL mode") -mav1.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) -mav2.recv_match(type='SYS_STATUS', condition='SYS_STATUS.mode==2 and SYS_STATUS.nav_mode==4', blocking=True) - -print("Setting declination") -mav1.mav.param_set_send(mav1.target_system, mav1.target_component, - 'COMPASS_DEC', radians(12.33)) -mav2.mav.param_set_send(mav2.target_system, mav2.target_component, - 'COMPASS_DEC', radians(12.33)) - - -set_attitude(1060, 1160) - -event = mavutil.periodic_event(30) -pevent = mavutil.periodic_event(0.3) -rc3_min = 1060 -rc3_max = 1850 -rc4_min = 1080 -rc4_max = 1500 -rc3 = rc3_min -rc4 = 1160 -delta3 = 2 -delta4 = 1 -use_pitch = 1 - -MAV_ACTION_CALIBRATE_GYRO = 17 -mav1.mav.action_send(mav1.target_system, mav1.target_component, MAV_ACTION_CALIBRATE_GYRO) -mav2.mav.action_send(mav2.target_system, mav2.target_component, MAV_ACTION_CALIBRATE_GYRO) - -print("Waiting for gyro calibration") -mav1.recv_match(type='ACTION_ACK') -mav2.recv_match(type='ACTION_ACK') - -print("Resetting mag offsets") -mav1.mav.set_mag_offsets_send(mav1.target_system, mav1.target_component, 0, 0, 0) -mav2.mav.set_mag_offsets_send(mav2.target_system, mav2.target_component, 0, 0, 0) - -def TrueHeading(SERVO_OUTPUT_RAW): - p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min) - return 172 + p*(326 - 172) - -while True: - mav1.recv_msg() - mav2.recv_msg() - if event.trigger(): - if not use_pitch: - rc4 = 1160 - set_attitude(rc3, rc4) - rc3 += delta3 - if rc3 > rc3_max or rc3 < rc3_min: - delta3 = -delta3 - use_pitch ^= 1 - rc4 += delta4 - if rc4 > rc4_max or rc4 < rc4_min: - delta4 = -delta4 - if pevent.trigger(): - print "hdg1: %3u hdg2: %3u ofs1: %4u, %4u, %4u ofs2: %4u, %4u, %4u" % ( - mav1.messages['VFR_HUD'].heading, - mav2.messages['VFR_HUD'].heading, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_x, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_y, - mav1.messages['SENSOR_OFFSETS'].mag_ofs_z, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_x, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_y, - mav2.messages['SENSOR_OFFSETS'].mag_ofs_z, - ) - time.sleep(0.01) - -# 314M 326G -# 160M 172G - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py b/mavlink/share/pyshared/pymavlink/examples/mavgraph.py deleted file mode 100644 index e19856487..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavgraph.py +++ /dev/null @@ -1,196 +0,0 @@ -#!/usr/bin/env python -''' -graph a MAVLink log file -Andrew Tridgell August 2011 -''' - -import sys, struct, time, os, datetime -import math, re -import pylab, pytz, matplotlib -from math import * - -# 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 mavextra import * - -locator = None -formatter = None - -def plotit(x, y, fields, colors=[]): - '''plot a set of graphs using date for x axis''' - global locator, formatter - pylab.ion() - fig = pylab.figure(num=1, figsize=(12,6)) - ax1 = fig.gca() - ax2 = None - xrange = 0.0 - for i in range(0, len(fields)): - if len(x[i]) == 0: continue - if x[i][-1] - x[i][0] > xrange: - xrange = x[i][-1] - x[i][0] - xrange *= 24 * 60 * 60 - if formatter is None: - if xrange < 1000: - formatter = matplotlib.dates.DateFormatter('%H:%M:%S') - else: - formatter = matplotlib.dates.DateFormatter('%H:%M') - interval = 1 - intervals = [ 1, 2, 5, 10, 15, 30, 60, 120, 240, 300, 600, - 900, 1800, 3600, 7200, 5*3600, 10*3600, 24*3600 ] - for interval in intervals: - if xrange / interval < 15: - break - locator = matplotlib.dates.SecondLocator(interval=interval) - ax1.xaxis.set_major_locator(locator) - ax1.xaxis.set_major_formatter(formatter) - empty = True - ax1_labels = [] - ax2_labels = [] - for i in range(0, len(fields)): - if len(x[i]) == 0: - print("Failed to find any values for field %s" % fields[i]) - continue - if i < len(colors): - color = colors[i] - else: - color = 'red' - (tz, tzdst) = time.tzname - if axes[i] == 2: - if ax2 == None: - ax2 = ax1.twinx() - ax = ax2 - ax2.xaxis.set_major_locator(locator) - ax2.xaxis.set_major_formatter(formatter) - label = fields[i] - if label.endswith(":2"): - label = label[:-2] - ax2_labels.append(label) - else: - ax1_labels.append(fields[i]) - ax = ax1 - ax.plot_date(x[i], y[i], color=color, label=fields[i], - linestyle='-', marker='None', tz=None) - pylab.draw() - empty = False - if ax1_labels != []: - ax1.legend(ax1_labels,loc=opts.legend) - if ax2_labels != []: - ax2.legend(ax2_labels,loc=opts.legend2) - if empty: - print("No data to graph") - return - - -from optparse import OptionParser -parser = OptionParser("mavgraph.py [options] <filename> <fields>") - -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") -parser.add_option("--labels",dest="labels", default=None, help="comma separated field labels") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--legend", default='upper left', help="default legend position") -parser.add_option("--legend2", default='upper right', help="default legend2 position") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 2: - print("Usage: mavlogdump.py [options] <LOGFILES...> <fields...>") - sys.exit(1) - -filenames = [] -fields = [] -for f in args: - if os.path.exists(f): - filenames.append(f) - else: - fields.append(f) -msg_types = set() -multiplier = [] -field_types = [] - -colors = [ 'red', 'green', 'blue', 'orange', 'olive', 'black', 'grey' ] - -# work out msg types we are interested in -x = [] -y = [] -axes = [] -first_only = [] -re_caps = re.compile('[A-Z_]+') -for f in fields: - caps = set(re.findall(re_caps, f)) - msg_types = msg_types.union(caps) - field_types.append(caps) - y.append([]) - x.append([]) - axes.append(1) - first_only.append(False) - -def add_data(t, msg, vars): - '''add some data''' - mtype = msg.get_type() - if mtype not in msg_types: - return - for i in range(0, len(fields)): - if mtype not in field_types[i]: - continue - f = fields[i] - if f.endswith(":2"): - axes[i] = 2 - f = f[:-2] - if f.endswith(":1"): - first_only[i] = True - f = f[:-2] - v = mavutil.evaluate_expression(f, vars) - if v is None: - continue - y[i].append(v) - x[i].append(t) - -def process_file(filename): - '''process one file''' - print("Processing %s" % filename) - mlog = mavutil.mavlink_connection(filename, notimestamps=opts.notimestamps) - vars = {} - - while True: - msg = mlog.recv_match(opts.condition) - if msg is None: break - tdays = (msg._timestamp - time.timezone) / (24 * 60 * 60) - tdays += 719163 # pylab wants it since 0001-01-01 - add_data(tdays, msg, mlog.messages) - -if len(filenames) == 0: - print("No files to process") - sys.exit(1) - -if opts.labels is not None: - labels = opts.labels.split(',') - if len(labels) != len(fields)*len(filenames): - print("Number of labels (%u) must match number of fields (%u)" % ( - len(labels), len(fields)*len(filenames))) - sys.exit(1) -else: - labels = None - -for fi in range(0, len(filenames)): - f = filenames[fi] - process_file(f) - for i in range(0, len(x)): - if first_only[i] and fi != 0: - x[i] = [] - y[i] = [] - if labels: - lab = labels[fi*len(fields):(fi+1)*len(fields)] - else: - lab = fields[:] - plotit(x, y, lab, colors=colors[fi*len(fields):]) - for i in range(0, len(x)): - x[i] = [] - y[i] = [] -pylab.show() -raw_input('press enter to exit....') diff --git a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py b/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py deleted file mode 100644 index f4cdc56bf..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -''' -example program that dumps a Mavlink log file. The log file is -assumed to be in the format that qgroundcontrol uses, which consists -of a series of MAVLink packets, each with a 64 bit timestamp -header. The timestamp is in microseconds since 1970 (unix epoch) -''' - -import sys, time, os, struct - -# 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("mavlogdump.py [options]") - -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") -parser.add_option("-f", "--follow",dest="follow", action='store_true', help="keep waiting for more data at end of file") -parser.add_option("--condition",dest="condition", default=None, help="select packets by condition") -parser.add_option("-q", "--quiet", dest="quiet", action='store_true', help="don't display packets") -parser.add_option("-o", "--output", default=None, help="output matching packets to give file") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--types", default=None, help="types of messages (comma separated)") -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: mavlogdump.py [options] <LOGFILE>") - sys.exit(1) - -filename = args[0] -mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner, - notimestamps=opts.notimestamps, - robust_parsing=opts.robust) - -output = None -if opts.output: - output = mavutil.mavlogfile(opts.output, write=True) - -types = opts.types -if types is not None: - types = types.split(',') - -while True: - m = mlog.recv_match(condition=opts.condition, blocking=opts.follow) - if m is None: - break - if types is not None and m.get_type() not in types: - continue - if output: - timestamp = getattr(m, '_timestamp', None) - if timestamp: - output.write(struct.pack('>Q', timestamp*1.0e6)) - output.write(m.get_msgbuf().tostring()) - if opts.quiet: - continue - print("%s.%02u: %s" % ( - time.strftime("%Y-%m-%d %H:%M:%S", - time.localtime(m._timestamp)), - int(m._timestamp*100.0)%100, m)) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavparms.py b/mavlink/share/pyshared/pymavlink/examples/mavparms.py deleted file mode 100644 index 702fbd9e1..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavparms.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -''' -extract mavlink parameter values -''' - -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("mavparms.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: mavparms.py [options] <LOGFILE...>") - sys.exit(1) - -parms = {} - -def mavparms(logfile): - '''extract mavlink parameters''' - mlog = mavutil.mavlink_connection(filename) - - while True: - m = mlog.recv_match(type='PARAM_VALUE') - if m is None: - return - pname = str(m.param_id).strip() - if len(pname) > 0: - parms[pname] = m.param_value - -total = 0.0 -for filename in args: - mavparms(filename) - -keys = parms.keys() -keys.sort() -for p in keys: - print("%-15s %.6f" % (p, parms[p])) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtest.py b/mavlink/share/pyshared/pymavlink/examples/mavtest.py deleted file mode 100644 index 3c385e48a..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtest.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env python - -import sys, 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__)), '..')) - -import mavlink - -class fifo(object): - def __init__(self): - self.buf = [] - def write(self, data): - self.buf += data - return len(data) - def read(self): - return self.buf.pop(0) - -f = fifo() - -# create a mavlink instance, which will do IO on file object 'f' -mav = mavlink.MAVLink(f) - -# set the WP_RADIUS parameter on the MAV at the end of the link -mav.param_set_send(7, 1, "WP_RADIUS", 101) - -# alternatively, produce a MAVLink_param_set object -# this can be sent via your own transport if you like -m = mav.param_set_encode(7, 1, "WP_RADIUS", 101) - -# get the encoded message as a buffer -b = m.get_msgbuf() - -# decode an incoming message -m2 = mav.decode(b) - -# show what fields it has -print("Got a message with id %u and fields %s" % (m2.get_msgId(), m2.get_fieldnames())) - -# print out the fields -print(m2) diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtester.py b/mavlink/share/pyshared/pymavlink/examples/mavtester.py deleted file mode 100644 index 8b5284f3f..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtester.py +++ /dev/null @@ -1,43 +0,0 @@ -#!/usr/bin/env python - -''' -test mavlink messages -''' - -import sys, struct, time, os -from curses import ascii - -# allow import from the parent directory, where mavlink.py is -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..')) - -import mavlink, mavtest, mavutil - -from optparse import OptionParser -parser = OptionParser("mavtester.py [options]") - -parser.add_option("--baudrate", dest="baudrate", type='int', - help="master port baud rate", default=115200) -parser.add_option("--device", dest="device", default=None, help="serial device") -parser.add_option("--source-system", dest='SOURCE_SYSTEM', type='int', - default=255, help='MAVLink source system for this GCS') -(opts, args) = parser.parse_args() - -if opts.device is None: - print("You must specify a serial device") - sys.exit(1) - -def wait_heartbeat(m): - '''wait for a heartbeat so we know the target system IDs''' - print("Waiting for APM heartbeat") - msg = m.recv_match(type='HEARTBEAT', blocking=True) - print("Heartbeat from APM (system %u component %u)" % (m.target_system, m.target_system)) - -# create a mavlink serial instance -master = mavutil.mavlink_connection(opts.device, baud=opts.baudrate, source_system=opts.SOURCE_SYSTEM) - -# wait for the heartbeat msg to find the system ID -wait_heartbeat(master) - -print("Sending all message types") -mavtest.generate_outputs(master.mav) - diff --git a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py b/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py deleted file mode 100644 index 92d3cb51c..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/mavtogpx.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python - -''' -example program to extract GPS data from a mavlink log, and create a GPX -file, for loading into google earth -''' - -import sys, struct, 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("mavtogpx.py [options]") -parser.add_option("--condition",dest="condition", default=None, help="select packets by a condition") -parser.add_option("--nofixcheck", default=False, action='store_true', help="don't check for GPS fix") -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: mavtogpx.py <LOGFILE>") - sys.exit(1) - -def mav_to_gpx(infilename, outfilename): - '''convert a mavlink log file to a GPX file''' - - mlog = mavutil.mavlink_connection(infilename) - outf = open(outfilename, mode='w') - - def process_packet(m): - t = time.localtime(m._timestamp) - outf.write('''<trkpt lat="%s" lon="%s"> - <ele>%s</ele> - <time>%s</time> - <course>%s</course> - <speed>%s</speed> - <fix>3d</fix> -</trkpt> -''' % (m.lat, m.lon, m.alt, - time.strftime("%Y-%m-%dT%H:%M:%SZ", t), - m.hdg, m.v)) - - def add_header(): - outf.write('''<?xml version="1.0" encoding="UTF-8"?> -<gpx - version="1.0" - creator="pymavlink" - xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" - xmlns="http://www.topografix.com/GPX/1/0" - xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd"> -<trk> -<trkseg> -''') - - def add_footer(): - outf.write('''</trkseg> -</trk> -</gpx> -''') - - add_header() - - count=0 - while True: - m = mlog.recv_match(type='GPS_RAW', condition=opts.condition) - if m is None: break - if m.fix_type != 2 and not opts.nofixcheck: - continue - if m.lat == 0.0 or m.lon == 0.0: - continue - process_packet(m) - count += 1 - add_footer() - print("Created %s with %u points" % (outfilename, count)) - - -for infilename in args: - outfilename = infilename + '.gpx' - mav_to_gpx(infilename, outfilename) diff --git a/mavlink/share/pyshared/pymavlink/examples/rotmat.py b/mavlink/share/pyshared/pymavlink/examples/rotmat.py deleted file mode 100644 index 6d5405949..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/rotmat.py +++ /dev/null @@ -1,269 +0,0 @@ -#!/usr/bin/env python -# -# vector3 and rotation matrix classes -# This follows the conventions in the ArduPilot code, -# and is essentially a python version of the AP_Math library -# -# Andrew Tridgell, March 2012 -# -# This library is free software; you can redistribute it and/or modify it -# under the terms of the GNU Lesser General Public License as published by the -# Free Software Foundation; either version 2.1 of the License, or (at your -# option) any later version. -# -# This library is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License -# for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with this library; if not, write to the Free Software Foundation, -# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - -'''rotation matrix class -''' - -from math import sin, cos, sqrt, asin, atan2, pi, radians, acos - -class Vector3: - '''a vector''' - def __init__(self, x=None, y=None, z=None): - if x != None and y != None and z != None: - self.x = float(x) - self.y = float(y) - self.z = float(z) - elif x != None and len(x) == 3: - self.x = float(x[0]) - self.y = float(x[1]) - self.z = float(x[2]) - elif x != None: - raise ValueError('bad initialiser') - else: - self.x = float(0) - self.y = float(0) - self.z = float(0) - - def __repr__(self): - return 'Vector3(%.2f, %.2f, %.2f)' % (self.x, - self.y, - self.z) - - def __add__(self, v): - return Vector3(self.x + v.x, - self.y + v.y, - self.z + v.z) - - __radd__ = __add__ - - def __sub__(self, v): - return Vector3(self.x - v.x, - self.y - v.y, - self.z - v.z) - - def __neg__(self): - return Vector3(-self.x, -self.y, -self.z) - - def __rsub__(self, v): - return Vector3(v.x - self.x, - v.y - self.y, - v.z - self.z) - - def __mul__(self, v): - if isinstance(v, Vector3): - '''dot product''' - return self.x*v.x + self.y*v.y + self.z*v.z - return Vector3(self.x * v, - self.y * v, - self.z * v) - - __rmul__ = __mul__ - - def __div__(self, v): - return Vector3(self.x / v, - self.y / v, - self.z / v) - - def __mod__(self, v): - '''cross product''' - return Vector3(self.y*v.z - self.z*v.y, - self.z*v.x - self.x*v.z, - self.x*v.y - self.y*v.x) - - def __copy__(self): - return Vector3(self.x, self.y, self.z) - - copy = __copy__ - - def length(self): - return sqrt(self.x**2 + self.y**2 + self.z**2) - - def zero(self): - self.x = self.y = self.z = 0 - - def angle(self, v): - '''return the angle between this vector and another vector''' - return acos(self * v) / (self.length() * v.length()) - - def normalized(self): - return self / self.length() - - def normalize(self): - v = self.normalized() - self.x = v.x - self.y = v.y - self.z = v.z - -class Matrix3: - '''a 3x3 matrix, intended as a rotation matrix''' - def __init__(self, a=None, b=None, c=None): - if a is not None and b is not None and c is not None: - self.a = a.copy() - self.b = b.copy() - self.c = c.copy() - else: - self.identity() - - def __repr__(self): - return 'Matrix3((%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f), (%.2f, %.2f, %.2f))' % ( - self.a.x, self.a.y, self.a.z, - self.b.x, self.b.y, self.b.z, - self.c.x, self.c.y, self.c.z) - - def identity(self): - self.a = Vector3(1,0,0) - self.b = Vector3(0,1,0) - self.c = Vector3(0,0,1) - - def transposed(self): - return Matrix3(Vector3(self.a.x, self.b.x, self.c.x), - Vector3(self.a.y, self.b.y, self.c.y), - Vector3(self.a.z, self.b.z, self.c.z)) - - - def from_euler(self, roll, pitch, yaw): - '''fill the matrix from Euler angles in radians''' - cp = cos(pitch) - sp = sin(pitch) - sr = sin(roll) - cr = cos(roll) - sy = sin(yaw) - cy = cos(yaw) - - self.a.x = cp * cy - self.a.y = (sr * sp * cy) - (cr * sy) - self.a.z = (cr * sp * cy) + (sr * sy) - self.b.x = cp * sy - self.b.y = (sr * sp * sy) + (cr * cy) - self.b.z = (cr * sp * sy) - (sr * cy) - self.c.x = -sp - self.c.y = sr * cp - self.c.z = cr * cp - - - def to_euler(self): - '''find Euler angles for the matrix''' - if self.c.x >= 1.0: - pitch = pi - elif self.c.x <= -1.0: - pitch = -pi - else: - pitch = -asin(self.c.x) - roll = atan2(self.c.y, self.c.z) - yaw = atan2(self.b.x, self.a.x) - return (roll, pitch, yaw) - - def __add__(self, m): - return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c) - - __radd__ = __add__ - - def __sub__(self, m): - return Matrix3(self.a - m.a, self.b - m.b, self.c - m.c) - - def __rsub__(self, m): - return Matrix3(m.a - self.a, m.b - self.b, m.c - self.c) - - def __mul__(self, other): - if isinstance(other, Vector3): - v = other - return Vector3(self.a.x * v.x + self.a.y * v.y + self.a.z * v.z, - self.b.x * v.x + self.b.y * v.y + self.b.z * v.z, - self.c.x * v.x + self.c.y * v.y + self.c.z * v.z) - elif isinstance(other, Matrix3): - m = other - return Matrix3(Vector3(self.a.x * m.a.x + self.a.y * m.b.x + self.a.z * m.c.x, - self.a.x * m.a.y + self.a.y * m.b.y + self.a.z * m.c.y, - self.a.x * m.a.z + self.a.y * m.b.z + self.a.z * m.c.z), - Vector3(self.b.x * m.a.x + self.b.y * m.b.x + self.b.z * m.c.x, - self.b.x * m.a.y + self.b.y * m.b.y + self.b.z * m.c.y, - self.b.x * m.a.z + self.b.y * m.b.z + self.b.z * m.c.z), - Vector3(self.c.x * m.a.x + self.c.y * m.b.x + self.c.z * m.c.x, - self.c.x * m.a.y + self.c.y * m.b.y + self.c.z * m.c.y, - self.c.x * m.a.z + self.c.y * m.b.z + self.c.z * m.c.z)) - v = other - return Matrix3(self.a * v, self.b * v, self.c * v) - - def __div__(self, v): - return Matrix3(self.a / v, self.b / v, self.c / v) - - def __neg__(self): - return Matrix3(-self.a, -self.b, -self.c) - - def __copy__(self): - return Matrix3(self.a, self.b, self.c) - - copy = __copy__ - - def rotate(self, g): - '''rotate the matrix by a given amount on 3 axes''' - temp_matrix = Matrix3() - a = self.a - b = self.b - c = self.c - temp_matrix.a.x = a.y * g.z - a.z * g.y - temp_matrix.a.y = a.z * g.x - a.x * g.z - temp_matrix.a.z = a.x * g.y - a.y * g.x - temp_matrix.b.x = b.y * g.z - b.z * g.y - temp_matrix.b.y = b.z * g.x - b.x * g.z - temp_matrix.b.z = b.x * g.y - b.y * g.x - temp_matrix.c.x = c.y * g.z - c.z * g.y - temp_matrix.c.y = c.z * g.x - c.x * g.z - temp_matrix.c.z = c.x * g.y - c.y * g.x - self.a += temp_matrix.a - self.b += temp_matrix.b - self.c += temp_matrix.c - - def normalize(self): - '''re-normalise a rotation matrix''' - error = self.a * self.b - t0 = self.a - (self.b * (0.5 * error)) - t1 = self.b - (self.a * (0.5 * error)) - t2 = t0 % t1 - self.a = t0 * (1.0 / t0.length()) - self.b = t1 * (1.0 / t1.length()) - self.c = t2 * (1.0 / t2.length()) - - def trace(self): - '''the trace of the matrix''' - return self.a.x + self.b.y + self.c.z - -def test_euler(): - '''check that from_euler() and to_euler() are consistent''' - m = Matrix3() - from math import radians, degrees - for r in range(-179, 179, 3): - for p in range(-89, 89, 3): - for y in range(-179, 179, 3): - m.from_euler(radians(r), radians(p), radians(y)) - (r2, p2, y2) = m.to_euler() - v1 = Vector3(r,p,y) - v2 = Vector3(degrees(r2),degrees(p2),degrees(y2)) - diff = v1 - v2 - if diff.length() > 1.0e-12: - print('EULER ERROR:', v1, v2, diff.length()) - -if __name__ == "__main__": - import doctest - doctest.testmod() - test_euler() - diff --git a/mavlink/share/pyshared/pymavlink/examples/sigloss.py b/mavlink/share/pyshared/pymavlink/examples/sigloss.py deleted file mode 100644 index feb189d97..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/sigloss.py +++ /dev/null @@ -1,57 +0,0 @@ -#!/usr/bin/env python - -''' -show times when signal is lost -''' - -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("sigloss.py [options]") -parser.add_option("--no-timestamps",dest="notimestamps", action='store_true', help="Log doesn't have timestamps") -parser.add_option("--planner",dest="planner", action='store_true', help="use planner file format") -parser.add_option("--robust",dest="robust", action='store_true', help="Enable robust parsing (skip over bad data)") -parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0") -parser.add_option("--deltat", type='float', default=1.0, help="loss threshold in seconds") - -(opts, args) = parser.parse_args() - -if opts.mav10: - os.environ['MAVLINK10'] = '1' -import mavutil - -if len(args) < 1: - print("Usage: sigloss.py [options] <LOGFILE...>") - sys.exit(1) - -def sigloss(logfile): - '''work out signal loss times for a log file''' - print("Processing log %s" % filename) - mlog = mavutil.mavlink_connection(filename, - planner_format=opts.planner, - notimestamps=opts.notimestamps, - robust_parsing=opts.robust) - - last_t = 0 - - while True: - m = mlog.recv_match() - if m is None: - return - if opts.notimestamps: - if not 'usec' in m._fieldnames: - continue - t = m.usec / 1.0e6 - else: - t = m._timestamp - if last_t != 0: - if t - last_t > opts.deltat: - print("Sig lost for %.1fs at %s" % (t-last_t, time.asctime(time.localtime(t)))) - last_t = t - -total = 0.0 -for filename in args: - sigloss(filename) diff --git a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py b/mavlink/share/pyshared/pymavlink/examples/wptogpx.py deleted file mode 100644 index 306f20af2..000000000 --- a/mavlink/share/pyshared/pymavlink/examples/wptogpx.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -''' -example program to extract GPS data from a waypoint file, and create a GPX -file, for loading into google earth -''' - -import sys, struct, 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("wptogpx.py [options]") -(opts, args) = parser.parse_args() - -import mavutil, mavwp - -if len(args) < 1: - print("Usage: wptogpx.py <WPFILE>") - sys.exit(1) - -def wp_to_gpx(infilename, outfilename): - '''convert a wp file to a GPX file''' - - wp = mavwp.MAVWPLoader() - wp.load(infilename) - outf = open(outfilename, mode='w') - - def process_wp(w, i): - t = time.localtime(i) - outf.write('''<wpt lat="%s" lon="%s"> - <ele>%s</ele> - <cmt>WP %u</cmt> -</wpt> -''' % (w.x, w.y, w.z, i)) - - def add_header(): - outf.write('''<?xml version="1.0" encoding="UTF-8"?> -<gpx - version="1.0" - creator="pymavlink" - xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" - xmlns="http://www.topografix.com/GPX/1/0" - xsi:schemaLocation="http://www.topografix.com/GPX/1/0 http://www.topografix.com/GPX/1/0/gpx.xsd"> -''') - - def add_footer(): - outf.write(''' -</gpx> -''') - - add_header() - - count = 0 - for i in range(wp.count()): - w = wp.wp(i) - if w.frame == 3: - w.z += wp.wp(0).z - if w.command == 16: - process_wp(w, i) - count += 1 - add_footer() - print("Created %s with %u points" % (outfilename, count)) - - -for infilename in args: - outfilename = infilename + '.gpx' - wp_to_gpx(infilename, outfilename) |