Skip to content
GitLab
Commit 79d121dc authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Remove non-needed files for MAVLink - we just want the C library

parent 3a385922
Expand all Hide whitespace changes
Inline Side-by-side
GLC_lib @ ef1adb08
Subproject commit ef1adb0843a9f8073bce9e641b2a0d9bf002ea39
libs/mavlink/lib/pkgconfig/mavlink.pc deleted 100644 → 0
View file @ 3a385922
prefix=/
exec_prefix=/
Name: mavlink
Description:
Version:
Cflags: -I//include/mavlink
libs/mavlink/share/pyshared/pymavlink/.gitignore deleted 100644 → 0
View file @ 3a385922
apidocs/
*.zip
*.pyc
send.sh
generator/C/include/ardupilotmega
generator/C/include/common
generator/C/include/pixhawk
generator/C/include/minimal
generator/C/include/ualberta
generator/C/include/slugs
testmav0.9*
testmav1.0*
Debug/
libs/mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde deleted 100644 → 0
View file @ 3a385922
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/*
send all possible mavlink messages
Andrew Tridgell July 2011
*/
// AVR runtime
#include <avr/io.h>
#include <avr/eeprom.h>
#include <avr/pgmspace.h>
#include <math.h>
// Libraries
#include <FastSerial.h>
#include <AP_Common.h>
#include <GCS_MAVLink.h>
#include "mavtest.h"
FastSerialPort0(Serial); // FTDI/console
FastSerialPort1(Serial1); // GPS port
FastSerialPort3(Serial3); // Telemetry port
#define SERIAL0_BAUD 115200
#define SERIAL3_BAUD 115200
void setup() {
Serial.begin(SERIAL0_BAUD, 128, 128);
Serial3.begin(SERIAL3_BAUD, 128, 128);
mavlink_comm_0_port = &Serial;
mavlink_comm_1_port = &Serial3;
}
void loop()
{
Serial.println("Starting MAVLink test generator\n");
while (1) {
mavlink_msg_heartbeat_send(
MAVLINK_COMM_0,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
mavlink_msg_heartbeat_send(
MAVLINK_COMM_1,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
mavtest_generate_outputs(MAVLINK_COMM_0);
mavtest_generate_outputs(MAVLINK_COMM_1);
delay(500);
}
}
libs/mavlink/share/pyshared/pymavlink/README.txt deleted 100644 → 0
View file @ 3a385922
This is a python implementation of the MAVLink protocol.
Please see http://www.qgroundcontrol.org/mavlink/pymavlink for
documentation
libs/mavlink/share/pyshared/pymavlink/examples/apmsetrate.py deleted 100644 → 0
View file @ 3a385922
#!/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)
libs/mavlink/share/pyshared/pymavlink/examples/bwtest.py deleted 100644 → 0
View file @ 3a385922
#!/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
libs/mavlink/share/pyshared/pymavlink/examples/flightmodes.py deleted 100644 → 0
View file @ 3a385922
#!/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)
libs/mavlink/share/pyshared/pymavlink/examples/flighttime.py deleted 100644 → 0
View file @ 3a385922
#!/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))
libs/mavlink/share/pyshared/pymavlink/examples/gpslock.py deleted 100644 → 0
View file @ 3a385922
#!/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))
libs/mavlink/share/pyshared/pymavlink/examples/magfit.py deleted 100644 → 0
View file @ 3a385922
#!/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)
libs/mavlink/share/pyshared/pymavlink/examples/magfit_delta.py deleted 100644 → 0
View file @ 3a385922
#!/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)
libs/mavlink/share/pyshared/pymavlink/examples/magfit_gps.py deleted 100644 → 0
View file @ 3a385922
#!/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)
libs/mavlink/share/pyshared/pymavlink/examples/magtest.py deleted 100644 → 0
View file @ 3a385922
#!/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
libs/mavlink/share/pyshared/pymavlink/examples/mavgraph.py deleted 100644 → 0
View file @ 3a385922
#!/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....')
libs/mavlink/share/pyshared/pymavlink/examples/mavlogdump.py deleted 100644 → 0
View file @ 3a385922
#!/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))
libs/mavlink/share/pyshared/pymavlink/examples/mavparms.py deleted 100644 → 0
View file @ 3a385922
#!/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]))
libs/mavlink/share/pyshared/pymavlink/examples/mavtest.py deleted 100644 → 0
View file @ 3a385922
#!/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)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment