fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 10:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

really remove mavlink folder

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-07 16:57:39 +02:00
parent 68dbf0057a
commit d79a80e8be
77 changed files with 0 additions and 34935 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mavlink/include/mavlink/config.h
-1
View File
@@ -1 +0,0 @@
#define MAVLINK_VERSION "1.0.7"
mavlink/include/mavlink/v1.0
-1
Submodule mavlink/include/mavlink/v1.0 deleted from 45a71d6564
mavlink/lib/pkgconfig/mavlink.pc
-7
View File
@@ -1,7 +0,0 @@
prefix=/
exec_prefix=/
Name: mavlink
Description:
Version:
Cflags: -I//include/mavlink
mavlink/share/mavlink/src/v1.0/pixhawk/pixhawk.pb.cc
-5431
View File
File diff suppressed because it is too large Load Diff
mavlink/share/pyshared/pymavlink/.gitignore
-13
View File
@@ -1,13 +0,0 @@
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/
mavlink/share/pyshared/pymavlink/APM_Mavtest/APM_Mavtest.pde
-55
View File
@@ -1,55 +0,0 @@
/// -*- 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);
}
}
mavlink/share/pyshared/pymavlink/README.txt
-4
View File
@@ -1,4 +0,0 @@
This is a python implementation of the MAVLink protocol.
Please see http://www.qgroundcontrol.org/mavlink/pymavlink for
documentation
mavlink/share/pyshared/pymavlink/examples/apmsetrate.py
-67
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/bwtest.py
-58
View File
@@ -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
mavlink/share/pyshared/pymavlink/examples/flightmodes.py
-52
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/flighttime.py
-59
View File
@@ -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))
mavlink/share/pyshared/pymavlink/examples/gpslock.py
-68
View File
@@ -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))
mavlink/share/pyshared/pymavlink/examples/magfit.py
-138
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/magfit_delta.py
-145
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/magfit_gps.py
-159
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/magtest.py
-120
View File
@@ -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
mavlink/share/pyshared/pymavlink/examples/mavgraph.py
-196
View File
@@ -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....')
mavlink/share/pyshared/pymavlink/examples/mavlogdump.py
-67
View File
@@ -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))
mavlink/share/pyshared/pymavlink/examples/mavparms.py
-48
View File
@@ -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]))
mavlink/share/pyshared/pymavlink/examples/mavtest.py
-41
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/mavtester.py
-43
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/mavtogpx.py
-83
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/rotmat.py
-269
View File
@@ -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()
mavlink/share/pyshared/pymavlink/examples/sigloss.py
-57
View File
@@ -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)
mavlink/share/pyshared/pymavlink/examples/wptogpx.py
-69
View File
@@ -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)
mavlink/share/pyshared/pymavlink/fgFDM.py
-209
View File
@@ -1,209 +0,0 @@
#!/usr/bin/env python
# parse and construct FlightGear NET FDM packets
# Andrew Tridgell, November 2011
# released under GNU GPL version 2 or later
import struct, math
class fgFDMError(Exception):
'''fgFDM error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = 'fgFDMError: ' + msg
class fgFDMVariable(object):
'''represent a single fgFDM variable'''
def __init__(self, index, arraylength, units):
self.index = index
self.arraylength = arraylength
self.units = units
class fgFDMVariableList(object):
'''represent a list of fgFDM variable'''
def __init__(self):
self.vars = {}
self._nextidx = 0
def add(self, varname, arraylength=1, units=None):
self.vars[varname] = fgFDMVariable(self._nextidx, arraylength, units=units)
self._nextidx += arraylength
class fgFDM(object):
'''a flightgear native FDM parser/generator'''
def __init__(self):
'''init a fgFDM object'''
self.FG_NET_FDM_VERSION = 24
self.pack_string = '>I 4x 3d 6f 11f 3f 2f I 4I 4f 4f 4f 4f 4f 4f 4f 4f 4f I 4f I 3I 3f 3f 3f I i f 10f'
self.values = [0]*98
self.FG_MAX_ENGINES = 4
self.FG_MAX_WHEELS = 3
self.FG_MAX_TANKS = 4
# supported unit mappings
self.unitmap = {
('radians', 'degrees') : math.degrees(1),
('rps', 'dps') : math.degrees(1),
('feet', 'meters') : 0.3048,
('fps', 'mps') : 0.3048,
('knots', 'mps') : 0.514444444,
('knots', 'fps') : 0.514444444/0.3048,
('fpss', 'mpss') : 0.3048,
('seconds', 'minutes') : 60,
('seconds', 'hours') : 3600,
}
# build a mapping between variable name and index in the values array
# note that the order of this initialisation is critical - it must
# match the wire structure
self.mapping = fgFDMVariableList()
self.mapping.add('version')
# position
self.mapping.add('longitude', units='radians') # geodetic (radians)
self.mapping.add('latitude', units='radians') # geodetic (radians)
self.mapping.add('altitude', units='meters') # above sea level (meters)
self.mapping.add('agl', units='meters') # above ground level (meters)
# attitude
self.mapping.add('phi', units='radians') # roll (radians)
self.mapping.add('theta', units='radians') # pitch (radians)
self.mapping.add('psi', units='radians') # yaw or true heading (radians)
self.mapping.add('alpha', units='radians') # angle of attack (radians)
self.mapping.add('beta', units='radians') # side slip angle (radians)
# Velocities
self.mapping.add('phidot', units='rps') # roll rate (radians/sec)
self.mapping.add('thetadot', units='rps') # pitch rate (radians/sec)
self.mapping.add('psidot', units='rps') # yaw rate (radians/sec)
self.mapping.add('vcas', units='fps') # calibrated airspeed
self.mapping.add('climb_rate', units='fps') # feet per second
self.mapping.add('v_north', units='fps') # north velocity in local/body frame, fps
self.mapping.add('v_east', units='fps') # east velocity in local/body frame, fps
self.mapping.add('v_down', units='fps') # down/vertical velocity in local/body frame, fps
self.mapping.add('v_wind_body_north', units='fps') # north velocity in local/body frame
self.mapping.add('v_wind_body_east', units='fps') # east velocity in local/body frame
self.mapping.add('v_wind_body_down', units='fps') # down/vertical velocity in local/body
# Accelerations
self.mapping.add('A_X_pilot', units='fpss') # X accel in body frame ft/sec^2
self.mapping.add('A_Y_pilot', units='fpss') # Y accel in body frame ft/sec^2
self.mapping.add('A_Z_pilot', units='fpss') # Z accel in body frame ft/sec^2
# Stall
self.mapping.add('stall_warning') # 0.0 - 1.0 indicating the amount of stall
self.mapping.add('slip_deg', units='degrees') # slip ball deflection
# Engine status
self.mapping.add('num_engines') # Number of valid engines
self.mapping.add('eng_state', self.FG_MAX_ENGINES) # Engine state (off, cranking, running)
self.mapping.add('rpm', self.FG_MAX_ENGINES) # Engine RPM rev/min
self.mapping.add('fuel_flow', self.FG_MAX_ENGINES) # Fuel flow gallons/hr
self.mapping.add('fuel_px', self.FG_MAX_ENGINES) # Fuel pressure psi
self.mapping.add('egt', self.FG_MAX_ENGINES) # Exhuast gas temp deg F
self.mapping.add('cht', self.FG_MAX_ENGINES) # Cylinder head temp deg F
self.mapping.add('mp_osi', self.FG_MAX_ENGINES) # Manifold pressure
self.mapping.add('tit', self.FG_MAX_ENGINES) # Turbine Inlet Temperature
self.mapping.add('oil_temp', self.FG_MAX_ENGINES) # Oil temp deg F
self.mapping.add('oil_px', self.FG_MAX_ENGINES) # Oil pressure psi
# Consumables
self.mapping.add('num_tanks') # Max number of fuel tanks
self.mapping.add('fuel_quantity', self.FG_MAX_TANKS)
# Gear status
self.mapping.add('num_wheels')
self.mapping.add('wow', self.FG_MAX_WHEELS)
self.mapping.add('gear_pos', self.FG_MAX_WHEELS)
self.mapping.add('gear_steer', self.FG_MAX_WHEELS)
self.mapping.add('gear_compression', self.FG_MAX_WHEELS)
# Environment
self.mapping.add('cur_time', units='seconds') # current unix time
self.mapping.add('warp', units='seconds') # offset in seconds to unix time
self.mapping.add('visibility', units='meters') # visibility in meters (for env. effects)
# Control surface positions (normalized values)
self.mapping.add('elevator')
self.mapping.add('elevator_trim_tab')
self.mapping.add('left_flap')
self.mapping.add('right_flap')
self.mapping.add('left_aileron')
self.mapping.add('right_aileron')
self.mapping.add('rudder')
self.mapping.add('nose_wheel')
self.mapping.add('speedbrake')
self.mapping.add('spoilers')
self._packet_size = struct.calcsize(self.pack_string)
self.set('version', self.FG_NET_FDM_VERSION)
if len(self.values) != self.mapping._nextidx:
raise fgFDMError('Invalid variable list in initialisation')
def packet_size(self):
'''return expected size of FG FDM packets'''
return self._packet_size
def convert(self, value, fromunits, tounits):
'''convert a value from one set of units to another'''
if fromunits == tounits:
return value
if (fromunits,tounits) in self.unitmap:
return value * self.unitmap[(fromunits,tounits)]
if (tounits,fromunits) in self.unitmap:
return value / self.unitmap[(tounits,fromunits)]
raise fgFDMError("unknown unit mapping (%s,%s)" % (fromunits, tounits))
def units(self, varname):
'''return the default units of a variable'''
if not varname in self.mapping.vars:
raise fgFDMError('Unknown variable %s' % varname)
return self.mapping.vars[varname].units
def variables(self):
'''return a list of available variables'''
return sorted(self.mapping.vars.keys(),
key = lambda v : self.mapping.vars[v].index)
def get(self, varname, idx=0, units=None):
'''get a variable value'''
if not varname in self.mapping.vars:
raise fgFDMError('Unknown variable %s' % varname)
if idx >= self.mapping.vars[varname].arraylength:
raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
varname, idx, self.mapping.vars[varname].arraylength))
value = self.values[self.mapping.vars[varname].index + idx]
if units:
value = self.convert(value, self.mapping.vars[varname].units, units)
return value
def set(self, varname, value, idx=0, units=None):
'''set a variable value'''
if not varname in self.mapping.vars:
raise fgFDMError('Unknown variable %s' % varname)
if idx >= self.mapping.vars[varname].arraylength:
raise fgFDMError('index of %s beyond end of array idx=%u arraylength=%u' % (
varname, idx, self.mapping.vars[varname].arraylength))
if units:
value = self.convert(value, units, self.mapping.vars[varname].units)
self.values[self.mapping.vars[varname].index + idx] = value
def parse(self, buf):
'''parse a FD FDM buffer'''
try:
t = struct.unpack(self.pack_string, buf)
except struct.error, msg:
raise fgFDMError('unable to parse - %s' % msg)
self.values = list(t)
def pack(self):
'''pack a FD FDM buffer from current values'''
for i in range(len(self.values)):
if math.isnan(self.values[i]):
self.values[i] = 0
return struct.pack(self.pack_string, *self.values)
mavlink/share/pyshared/pymavlink/generator/.gitignore
-1
View File
@@ -1 +0,0 @@
*.pyc
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/checksum.h
-89
View File
@@ -1,89 +0,0 @@
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _CHECKSUM_H_
#define _CHECKSUM_H_
/**
*
* CALCULATE THE CHECKSUM
*
*/
#define X25_INIT_CRC 0xffff
#define X25_VALIDATE_CRC 0xf0b8
/**
* @brief Accumulate the X.25 CRC by adding one char at a time.
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new char to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
/*Accumulate one byte of data into the CRC*/
uint8_t tmp;
tmp = data ^ (uint8_t)(*crcAccum &0xff);
tmp ^= (tmp<<4);
*crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
}
/**
* @brief Initiliaze the buffer for the X.25 CRC
*
* @param crcAccum the 16 bit X.25 CRC
*/
static inline void crc_init(uint16_t* crcAccum)
{
*crcAccum = X25_INIT_CRC;
}
/**
* @brief Calculates the X.25 checksum on a byte buffer
*
* @param pBuffer buffer containing the byte array to hash
* @param length length of the byte array
* @return the checksum over the buffer bytes
**/
static inline uint16_t crc_calculate(uint8_t* pBuffer, uint16_t length)
{
uint16_t crcTmp;
crc_init(&crcTmp);
while (length--) {
crc_accumulate(*pBuffer++, &crcTmp);
}
return crcTmp;
}
/**
* @brief Accumulate the X.25 CRC by adding an array of bytes
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new bytes to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
{
const uint8_t *p = (const uint8_t *)pBuffer;
while (length--) {
crc_accumulate(*p++, crcAccum);
}
}
#endif /* _CHECKSUM_H_ */
#ifdef __cplusplus
}
#endif
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_helpers.h
-488
View File
@@ -1,488 +0,0 @@
#ifndef _MAVLINK_HELPERS_H_
#define _MAVLINK_HELPERS_H_
#include "string.h"
#include "checksum.h"
#include "mavlink_types.h"
#ifndef MAVLINK_HELPER
#define MAVLINK_HELPER
#endif
/*
internal function to give access to the channel status for each channel
*/
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan];
}
/**
* @brief Finalize a MAVLink message with channel assignment
*
* This function calculates the checksum and sets length and aircraft id correctly.
* It assumes that the message id and the payload are already correctly set. This function
* can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
* instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
*
* @param msg Message to finalize
* @param system_id Id of the sending (this) system, 1-127
* @param length Message length
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length)
#endif
{
// This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX;
msg->len = length;
msg->sysid = system_id;
msg->compid = component_id;
// One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
/**
* @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length, uint8_t crc_extra)
{
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
}
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length)
{
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
/**
* @brief Finalize a MAVLink message with channel assignment and send
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
#endif
{
uint16_t checksum;
uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
uint8_t ck[2];
mavlink_status_t *status = mavlink_get_channel_status(chan);
buf[0] = MAVLINK_STX;
buf[1] = length;
buf[2] = status->current_tx_seq;
buf[3] = mavlink_system.sysid;
buf[4] = mavlink_system.compid;
buf[5] = msgid;
status->current_tx_seq++;
checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
#endif
ck[0] = (uint8_t)(checksum & 0xFF);
ck[1] = (uint8_t)(checksum >> 8);
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
_mavlink_send_uart(chan, packet, length);
_mavlink_send_uart(chan, (const char *)ck, 2);
MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
}
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
/**
* @brief Pack a message to send it over a serial byte stream
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}
union __mavlink_bitfield {
uint8_t uint8;
int8_t int8;
uint16_t uint16;
int16_t int16;
uint32_t uint32;
int32_t int32;
};
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
{
crc_init(&msg->checksum);
}
MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
{
crc_accumulate(c, &msg->checksum);
}
/**
* This is a convenience function which handles the complete MAVLink parsing.
* the function will parse one byte at a time and return the complete packet once
* it could be successfully decoded. Checksum and other failures will be silently
* ignored.
*
* @param chan ID of the current channel. This allows to parse different channels with this function.
* a channel is not a physical message channel like a serial port, but a logic partition of
* the communication streams in this case. COMM_NB is the limit for the number of channels
* on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
* @param c The char to barse
*
* @param returnMsg NULL if no message could be decoded, the message data else
* @return 0 if no message could be decoded, 1 else
*
* A typical use scenario of this function call is:
*
* @code
* #include <inttypes.h> // For fixed-width uint8_t type
*
* mavlink_message_t msg;
* int chan = 0;
*
*
* while(serial.bytesAvailable > 0)
* {
* uint8_t byte = serial.getNextByte();
* if (mavlink_parse_char(chan, byte, &msg))
* {
* printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
* }
* }
*
*
* @endcode
*/
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
/*
default message crc function. You can override this per-system to
put this data in a different memory segment
*/
#if MAVLINK_CRC_EXTRA
#ifndef MAVLINK_MESSAGE_CRC
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
#endif
#endif
mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message
mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
int bufferIndex = 0;
status->msg_received = 0;
switch (status->parse_state)
{
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
if (status->msg_received
/* Support shorter buffers than the
default maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|| c > MAVLINK_MAX_PAYLOAD_LEN
#endif
)
{
status->buffer_overrun++;
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
}
else
{
// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
rxmsg->len = c;
status->packet_idx = 0;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
}
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
rxmsg->seq = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
rxmsg->sysid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
rxmsg->compid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
rxmsg->msgid = c;
mavlink_update_checksum(rxmsg, c);
if (rxmsg->len == 0)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
else
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID:
_MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c;
mavlink_update_checksum(rxmsg, c);
if (status->packet_idx == rxmsg->len)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
#if MAVLINK_CRC_EXTRA
mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
#endif
if (c != (rxmsg->checksum & 0xFF)) {
// Check first checksum byte
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
}
else
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC1:
if (c != (rxmsg->checksum >> 8)) {
// Check second checksum byte
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
}
else
{
// Successfully got message
status->msg_received = 1;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
break;
}
bufferIndex++;
// If a message has been sucessfully decoded, check index
if (status->msg_received == 1)
{
//while(status->current_seq != rxmsg->seq)
//{
// status->packet_rx_drop_count++;
// status->current_seq++;
//}
status->current_rx_seq = rxmsg->seq;
// Initial condition: If no packet has been received so far, drop count is undefined
if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
// Count this packet as received
status->packet_rx_success_count++;
}
r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
r_mavlink_status->packet_rx_drop_count = status->parse_error;
status->parse_error = 0;
return status->msg_received;
}
/**
* @brief Put a bitfield of length 1-32 bit into the buffer
*
* @param b the value to add, will be encoded in the bitfield
* @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
* @param packet_index the position in the packet (the index of the first byte to use)
* @param bit_index the position in the byte (the index of the first bit to use)
* @param buffer packet buffer to write into
* @return new position of the last used byte in the buffer
*/
MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
{
uint16_t bits_remain = bits;
// Transform number into network order
int32_t v;
uint8_t i_bit_index, i_byte_index, curr_bits_n;
#if MAVLINK_NEED_BYTE_SWAP
union {
int32_t i;
uint8_t b[4];
} bin, bout;
bin.i = b;
bout.b[0] = bin.b[3];
bout.b[1] = bin.b[2];
bout.b[2] = bin.b[1];
bout.b[3] = bin.b[0];
v = bout.i;
#else
v = b;
#endif
// buffer in
// 01100000 01000000 00000000 11110001
// buffer out
// 11110001 00000000 01000000 01100000
// Existing partly filled byte (four free slots)
// 0111xxxx
// Mask n free bits
// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
// Shift n bits into the right position
// out = in >> n;
// Mask and shift bytes
i_bit_index = bit_index;
i_byte_index = packet_index;
if (bit_index > 0)
{
// If bits were available at start, they were available
// in the byte before the current index
i_byte_index--;
}
// While bits have not been packed yet
while (bits_remain > 0)
{
// Bits still have to be packed
// there can be more than 8 bits, so
// we might have to pack them into more than one byte
// First pack everything we can into the current 'open' byte
//curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
//FIXME
if (bits_remain <= (uint8_t)(8 - i_bit_index))
{
// Enough space
curr_bits_n = (uint8_t)bits_remain;
}
else
{
curr_bits_n = (8 - i_bit_index);
}
// Pack these n bits into the current byte
// Mask out whatever was at that position with ones (xxx11111)
buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
// Put content to this position, by masking out the non-used part
buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
// Increment the bit index
i_bit_index += curr_bits_n;
// Now proceed to the next byte, if necessary
bits_remain -= curr_bits_n;
if (bits_remain > 0)
{
// Offer another 8 bits / one byte
i_byte_index++;
i_bit_index = 0;
}
}
*r_bit_index = i_bit_index;
// If a partly filled byte is present, mark this as consumed
if (i_bit_index != 7) i_byte_index++;
return i_byte_index - packet_index;
}
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
// To make MAVLink work on your MCU, define comm_send_ch() if you wish
// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
// whole packet at a time
/*
#include "mavlink_types.h"
void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
if (chan == MAVLINK_COMM_0)
{
uart0_transmit(ch);
}
if (chan == MAVLINK_COMM_1)
{
uart1_transmit(ch);
}
}
*/
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
{
#ifdef MAVLINK_SEND_UART_BYTES
/* this is the more efficient approach, if the platform
defines it */
MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len);
#else
/* fallback to one byte at a time */
uint16_t i;
for (i = 0; i < len; i++) {
comm_send_ch(chan, (uint8_t)buf[i]);
}
#endif
}
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
#endif /* _MAVLINK_HELPERS_H_ */
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/mavlink_types.h
-300
View File
@@ -1,300 +0,0 @@
#ifndef MAVLINK_TYPES_H_
#define MAVLINK_TYPES_H_
#include "inttypes.h"
enum MAV_CLASS
{
MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything
MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch
MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu
MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com
MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org
MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints
MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands
MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set
MAV_CLASS_NONE = 8, ///< No valid autopilot
MAV_CLASS_NB ///< Number of autopilot classes
};
enum MAV_ACTION
{
MAV_ACTION_HOLD = 0,
MAV_ACTION_MOTORS_START = 1,
MAV_ACTION_LAUNCH = 2,
MAV_ACTION_RETURN = 3,
MAV_ACTION_EMCY_LAND = 4,
MAV_ACTION_EMCY_KILL = 5,
MAV_ACTION_CONFIRM_KILL = 6,
MAV_ACTION_CONTINUE = 7,
MAV_ACTION_MOTORS_STOP = 8,
MAV_ACTION_HALT = 9,
MAV_ACTION_SHUTDOWN = 10,
MAV_ACTION_REBOOT = 11,
MAV_ACTION_SET_MANUAL = 12,
MAV_ACTION_SET_AUTO = 13,
MAV_ACTION_STORAGE_READ = 14,
MAV_ACTION_STORAGE_WRITE = 15,
MAV_ACTION_CALIBRATE_RC = 16,
MAV_ACTION_CALIBRATE_GYRO = 17,
MAV_ACTION_CALIBRATE_MAG = 18,
MAV_ACTION_CALIBRATE_ACC = 19,
MAV_ACTION_CALIBRATE_PRESSURE = 20,
MAV_ACTION_REC_START = 21,
MAV_ACTION_REC_PAUSE = 22,
MAV_ACTION_REC_STOP = 23,
MAV_ACTION_TAKEOFF = 24,
MAV_ACTION_NAVIGATE = 25,
MAV_ACTION_LAND = 26,
MAV_ACTION_LOITER = 27,
MAV_ACTION_SET_ORIGIN = 28,
MAV_ACTION_RELAY_ON = 29,
MAV_ACTION_RELAY_OFF = 30,
MAV_ACTION_GET_IMAGE = 31,
MAV_ACTION_VIDEO_START = 32,
MAV_ACTION_VIDEO_STOP = 33,
MAV_ACTION_RESET_MAP = 34,
MAV_ACTION_RESET_PLAN = 35,
MAV_ACTION_DELAY_BEFORE_COMMAND = 36,
MAV_ACTION_ASCEND_AT_RATE = 37,
MAV_ACTION_CHANGE_MODE = 38,
MAV_ACTION_LOITER_MAX_TURNS = 39,
MAV_ACTION_LOITER_MAX_TIME = 40,
MAV_ACTION_START_HILSIM = 41,
MAV_ACTION_STOP_HILSIM = 42,
MAV_ACTION_NB ///< Number of MAV actions
};
enum MAV_MODE
{
MAV_MODE_UNINIT = 0, ///< System is in undefined state
MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe
MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control
MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint
MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation
MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use
MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use
MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use
MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive
MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back
};
enum MAV_STATE
{
MAV_STATE_UNINIT = 0,
MAV_STATE_BOOT,
MAV_STATE_CALIBRATING,
MAV_STATE_STANDBY,
MAV_STATE_ACTIVE,
MAV_STATE_CRITICAL,
MAV_STATE_EMERGENCY,
MAV_STATE_HILSIM,
MAV_STATE_POWEROFF
};
enum MAV_NAV
{
MAV_NAV_GROUNDED = 0,
MAV_NAV_LIFTOFF,
MAV_NAV_HOLD,
MAV_NAV_WAYPOINT,
MAV_NAV_VECTOR,
MAV_NAV_RETURNING,
MAV_NAV_LANDING,
MAV_NAV_LOST,
MAV_NAV_LOITER,
MAV_NAV_FREE_DRIFT
};
enum MAV_TYPE
{
MAV_GENERIC = 0,
MAV_FIXED_WING = 1,
MAV_QUADROTOR = 2,
MAV_COAXIAL = 3,
MAV_HELICOPTER = 4,
MAV_GROUND = 5,
OCU = 6,
MAV_AIRSHIP = 7,
MAV_FREE_BALLOON = 8,
MAV_ROCKET = 9,
UGV_GROUND_ROVER = 10,
UGV_SURFACE_SHIP = 11
};
enum MAV_AUTOPILOT_TYPE
{
MAV_AUTOPILOT_GENERIC = 0,
MAV_AUTOPILOT_PIXHAWK = 1,
MAV_AUTOPILOT_SLUGS = 2,
MAV_AUTOPILOT_ARDUPILOTMEGA = 3,
MAV_AUTOPILOT_NONE = 4
};
enum MAV_COMPONENT
{
MAV_COMP_ID_GPS,
MAV_COMP_ID_WAYPOINTPLANNER,
MAV_COMP_ID_BLOBTRACKER,
MAV_COMP_ID_PATHPLANNER,
MAV_COMP_ID_AIRSLAM,
MAV_COMP_ID_MAPPER,
MAV_COMP_ID_CAMERA,
MAV_COMP_ID_RADIO = 68,
MAV_COMP_ID_IMU = 200,
MAV_COMP_ID_IMU_2 = 201,
MAV_COMP_ID_IMU_3 = 202,
MAV_COMP_ID_UDP_BRIDGE = 240,
MAV_COMP_ID_UART_BRIDGE = 241,
MAV_COMP_ID_SYSTEM_CONTROL = 250
};
enum MAV_FRAME
{
MAV_FRAME_GLOBAL = 0,
MAV_FRAME_LOCAL = 1,
MAV_FRAME_MISSION = 2,
MAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
MAV_FRAME_LOCAL_ENU = 4
};
enum MAVLINK_DATA_STREAM_TYPE
{
MAVLINK_DATA_STREAM_IMG_JPEG,
MAVLINK_DATA_STREAM_IMG_BMP,
MAVLINK_DATA_STREAM_IMG_RAW8U,
MAVLINK_DATA_STREAM_IMG_RAW32U,
MAVLINK_DATA_STREAM_IMG_PGM,
MAVLINK_DATA_STREAM_IMG_PNG
};
#ifndef MAVLINK_MAX_PAYLOAD_LEN
// it is possible to override this, but be careful!
#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
#endif
#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
#define MAVLINK_NUM_CHECKSUM_BYTES 2
#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
typedef struct param_union {
union {
float param_float;
int32_t param_int32;
uint32_t param_uint32;
};
uint8_t type;
} mavlink_param_union_t;
typedef struct __mavlink_system {
uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function
uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function
uint8_t type; ///< Unused, can be used by user to store the system's type
uint8_t state; ///< Unused, can be used by user to store the system's state
uint8_t mode; ///< Unused, can be used by user to store the system's mode
uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
} mavlink_system_t;
typedef struct __mavlink_message {
uint16_t checksum; /// sent at end of packet
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint8_t msgid; ///< ID of message in payload
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
typedef enum {
MAVLINK_TYPE_CHAR = 0,
MAVLINK_TYPE_UINT8_T = 1,
MAVLINK_TYPE_INT8_T = 2,
MAVLINK_TYPE_UINT16_T = 3,
MAVLINK_TYPE_INT16_T = 4,
MAVLINK_TYPE_UINT32_T = 5,
MAVLINK_TYPE_INT32_T = 6,
MAVLINK_TYPE_UINT64_T = 7,
MAVLINK_TYPE_INT64_T = 8,
MAVLINK_TYPE_FLOAT = 9,
MAVLINK_TYPE_DOUBLE = 10
} mavlink_message_type_t;
#define MAVLINK_MAX_FIELDS 64
typedef struct __mavlink_field_info {
const char *name; // name of this field
const char *print_format; // printing format hint, or NULL
mavlink_message_type_t type; // type of this field
unsigned array_length; // if non-zero, field is an array
unsigned wire_offset; // offset of each field in the payload
unsigned structure_offset; // offset in a C structure
} mavlink_field_info_t;
// note that in this structure the order of fields is the order
// in the XML file, not necessary the wire order
typedef struct __mavlink_message_info {
const char *name; // name of the message
unsigned num_fields; // how many fields in this message
mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
} mavlink_message_info_t;
#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0]))
#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
// checksum is immediately after the payload bytes
#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg))
#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg))
typedef enum {
MAVLINK_COMM_0,
MAVLINK_COMM_1,
MAVLINK_COMM_2,
MAVLINK_COMM_3
} mavlink_channel_t;
/*
* applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
* of buffers they will use. If more are used, then the result will be
* a stack overrun
*/
#ifndef MAVLINK_COMM_NUM_BUFFERS
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
# define MAVLINK_COMM_NUM_BUFFERS 16
#else
# define MAVLINK_COMM_NUM_BUFFERS 4
#endif
#endif
typedef enum {
MAVLINK_PARSE_STATE_UNINIT=0,
MAVLINK_PARSE_STATE_IDLE,
MAVLINK_PARSE_STATE_GOT_STX,
MAVLINK_PARSE_STATE_GOT_SEQ,
MAVLINK_PARSE_STATE_GOT_LENGTH,
MAVLINK_PARSE_STATE_GOT_SYSID,
MAVLINK_PARSE_STATE_GOT_COMPID,
MAVLINK_PARSE_STATE_GOT_MSGID,
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_CRC1
} mavlink_parse_state_t; ///< The state machine for the comm parser
typedef struct __mavlink_status {
uint8_t msg_received; ///< Number of received messages
uint8_t buffer_overrun; ///< Number of buffer overruns
uint8_t parse_error; ///< Number of parse errors
mavlink_parse_state_t parse_state; ///< Parsing state machine
uint8_t packet_idx; ///< Index in current packet
uint8_t current_rx_seq; ///< Sequence number of last packet received
uint8_t current_tx_seq; ///< Sequence number of last packet sent
uint16_t packet_rx_success_count; ///< Received packets
uint16_t packet_rx_drop_count; ///< Number of packet drops
} mavlink_status_t;
#define MAVLINK_BIG_ENDIAN 0
#define MAVLINK_LITTLE_ENDIAN 1
#endif /* MAVLINK_TYPES_H_ */
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/protocol.h
-319
View File
@@ -1,319 +0,0 @@
#ifndef _MAVLINK_PROTOCOL_H_
#define _MAVLINK_PROTOCOL_H_
#include "string.h"
#include "mavlink_types.h"
/*
If you want MAVLink on a system that is native big-endian,
you need to define NATIVE_BIG_ENDIAN
*/
#ifdef NATIVE_BIG_ENDIAN
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
#else
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
#endif
#ifndef MAVLINK_STACK_BUFFER
#define MAVLINK_STACK_BUFFER 0
#endif
#ifndef MAVLINK_AVOID_GCC_STACK_BUG
# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
#endif
#ifndef MAVLINK_ASSERT
#define MAVLINK_ASSERT(x)
#endif
#ifndef MAVLINK_START_UART_SEND
#define MAVLINK_START_UART_SEND(chan, length)
#endif
#ifndef MAVLINK_END_UART_SEND
#define MAVLINK_END_UART_SEND(chan, length)
#endif
#ifdef MAVLINK_SEPARATE_HELPERS
#define MAVLINK_HELPER
#else
#define MAVLINK_HELPER static inline
#include "mavlink_helpers.h"
#endif // MAVLINK_SEPARATE_HELPERS
/* always include the prototypes to ensure we don't get out of sync */
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length, uint8_t crc_extra);
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
uint8_t length, uint8_t crc_extra);
#endif
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length);
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
#endif // MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
uint8_t* r_bit_index, uint8_t* buffer);
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
#endif
/**
* @brief Get the required buffer size for this message
*/
static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
{
return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
#if MAVLINK_NEED_BYTE_SWAP
static inline void byte_swap_2(char *dst, const char *src)
{
dst[0] = src[1];
dst[1] = src[0];
}
static inline void byte_swap_4(char *dst, const char *src)
{
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
}
static inline void byte_swap_8(char *dst, const char *src)
{
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
}
#elif !MAVLINK_ALIGNED_FIELDS
static inline void byte_copy_2(char *dst, const char *src)
{
dst[0] = src[0];
dst[1] = src[1];
}
static inline void byte_copy_4(char *dst, const char *src)
{
dst[0] = src[0];
dst[1] = src[1];
dst[2] = src[2];
dst[3] = src[3];
}
static inline void byte_copy_8(char *dst, const char *src)
{
memcpy(dst, src, 8);
}
#endif
#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b
#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b
#if MAVLINK_NEED_BYTE_SWAP
#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#elif !MAVLINK_ALIGNED_FIELDS
#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#else
#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b
#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b
#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b
#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b
#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b
#endif
/*
like memcpy(), but if src is NULL, do a memset to zero
*/
static void mav_array_memcpy(void *dest, const void *src, size_t n)
{
if (src == NULL) {
memset(dest, 0, n);
} else {
memcpy(dest, src, n);
}
}
/*
* Place a char array into a buffer
*/
static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
* Place a uint8_t array into a buffer
*/
static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
* Place a int8_t array into a buffer
*/
static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_PUT_ARRAY(TYPE, V) \
static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
{ \
if (b == NULL) { \
memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
} else { \
uint16_t i; \
for (i=0; i<array_length; i++) { \
_mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
} \
} \
}
#else
#define _MAV_PUT_ARRAY(TYPE, V) \
static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
{ \
mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
}
#endif
_MAV_PUT_ARRAY(uint16_t, u16)
_MAV_PUT_ARRAY(uint32_t, u32)
_MAV_PUT_ARRAY(uint64_t, u64)
_MAV_PUT_ARRAY(int16_t, i16)
_MAV_PUT_ARRAY(int32_t, i32)
_MAV_PUT_ARRAY(int64_t, i64)
_MAV_PUT_ARRAY(float, f)
_MAV_PUT_ARRAY(double, d)
#define _MAV_RETURN_char(msg, wire_offset) _MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#elif !MAVLINK_ALIGNED_FIELDS
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#else // nicely aligned, no swap
#define _MAV_MSG_RETURN_TYPE(TYPE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
_MAV_MSG_RETURN_TYPE(uint16_t)
_MAV_MSG_RETURN_TYPE(int16_t)
_MAV_MSG_RETURN_TYPE(uint32_t)
_MAV_MSG_RETURN_TYPE(int32_t)
_MAV_MSG_RETURN_TYPE(uint64_t)
_MAV_MSG_RETURN_TYPE(int64_t)
_MAV_MSG_RETURN_TYPE(float)
_MAV_MSG_RETURN_TYPE(double)
#endif // MAVLINK_NEED_BYTE_SWAP
static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
uint16_t i; \
for (i=0; i<array_length; i++) { \
value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
} \
return array_length*sizeof(value[0]); \
}
#else
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
return array_length*sizeof(TYPE); \
}
#endif
_MAV_RETURN_ARRAY(uint16_t, u16)
_MAV_RETURN_ARRAY(uint32_t, u32)
_MAV_RETURN_ARRAY(uint64_t, u64)
_MAV_RETURN_ARRAY(int16_t, i16)
_MAV_RETURN_ARRAY(int32_t, i32)
_MAV_RETURN_ARRAY(int64_t, i64)
_MAV_RETURN_ARRAY(float, f)
_MAV_RETURN_ARRAY(double, d)
#endif // _MAVLINK_PROTOCOL_H_
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink.h
-27
View File
@@ -1,27 +0,0 @@
/** @file
* @brief MAVLink comm protocol built from test.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#ifndef MAVLINK_STX
#define MAVLINK_STX 85
#endif
#ifndef MAVLINK_ENDIAN
#define MAVLINK_ENDIAN MAVLINK_BIG_ENDIAN
#endif
#ifndef MAVLINK_ALIGNED_FIELDS
#define MAVLINK_ALIGNED_FIELDS 0
#endif
#ifndef MAVLINK_CRC_EXTRA
#define MAVLINK_CRC_EXTRA 0
#endif
#include "version.h"
#include "test.h"
#endif // MAVLINK_H
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/mavlink_msg_test_types.h
-610
View File
@@ -1,610 +0,0 @@
// MESSAGE TEST_TYPES PACKING
#define MAVLINK_MSG_ID_TEST_TYPES 0
typedef struct __mavlink_test_types_t
{
char c; ///< char
char s[10]; ///< string
uint8_t u8; ///< uint8_t
uint16_t u16; ///< uint16_t
uint32_t u32; ///< uint32_t
uint64_t u64; ///< uint64_t
int8_t s8; ///< int8_t
int16_t s16; ///< int16_t
int32_t s32; ///< int32_t
int64_t s64; ///< int64_t
float f; ///< float
double d; ///< double
uint8_t u8_array[3]; ///< uint8_t_array
uint16_t u16_array[3]; ///< uint16_t_array
uint32_t u32_array[3]; ///< uint32_t_array
uint64_t u64_array[3]; ///< uint64_t_array
int8_t s8_array[3]; ///< int8_t_array
int16_t s16_array[3]; ///< int16_t_array
int32_t s32_array[3]; ///< int32_t_array
int64_t s64_array[3]; ///< int64_t_array
float f_array[3]; ///< float_array
double d_array[3]; ///< double_array
} mavlink_test_types_t;
#define MAVLINK_MSG_ID_TEST_TYPES_LEN 179
#define MAVLINK_MSG_ID_0_LEN 179
#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10
#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_U64_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S64_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_D_ARRAY_LEN 3
#define MAVLINK_MESSAGE_INFO_TEST_TYPES { \
"TEST_TYPES", \
22, \
{ { "c", NULL, MAVLINK_TYPE_CHAR, 0, 0, offsetof(mavlink_test_types_t, c) }, \
{ "s", NULL, MAVLINK_TYPE_CHAR, 10, 1, offsetof(mavlink_test_types_t, s) }, \
{ "u8", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_test_types_t, u8) }, \
{ "u16", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_test_types_t, u16) }, \
{ "u32", "0x%08x", MAVLINK_TYPE_UINT32_T, 0, 14, offsetof(mavlink_test_types_t, u32) }, \
{ "u64", NULL, MAVLINK_TYPE_UINT64_T, 0, 18, offsetof(mavlink_test_types_t, u64) }, \
{ "s8", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_test_types_t, s8) }, \
{ "s16", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_test_types_t, s16) }, \
{ "s32", NULL, MAVLINK_TYPE_INT32_T, 0, 29, offsetof(mavlink_test_types_t, s32) }, \
{ "s64", NULL, MAVLINK_TYPE_INT64_T, 0, 33, offsetof(mavlink_test_types_t, s64) }, \
{ "f", NULL, MAVLINK_TYPE_FLOAT, 0, 41, offsetof(mavlink_test_types_t, f) }, \
{ "d", NULL, MAVLINK_TYPE_DOUBLE, 0, 45, offsetof(mavlink_test_types_t, d) }, \
{ "u8_array", NULL, MAVLINK_TYPE_UINT8_T, 3, 53, offsetof(mavlink_test_types_t, u8_array) }, \
{ "u16_array", NULL, MAVLINK_TYPE_UINT16_T, 3, 56, offsetof(mavlink_test_types_t, u16_array) }, \
{ "u32_array", NULL, MAVLINK_TYPE_UINT32_T, 3, 62, offsetof(mavlink_test_types_t, u32_array) }, \
{ "u64_array", NULL, MAVLINK_TYPE_UINT64_T, 3, 74, offsetof(mavlink_test_types_t, u64_array) }, \
{ "s8_array", NULL, MAVLINK_TYPE_INT8_T, 3, 98, offsetof(mavlink_test_types_t, s8_array) }, \
{ "s16_array", NULL, MAVLINK_TYPE_INT16_T, 3, 101, offsetof(mavlink_test_types_t, s16_array) }, \
{ "s32_array", NULL, MAVLINK_TYPE_INT32_T, 3, 107, offsetof(mavlink_test_types_t, s32_array) }, \
{ "s64_array", NULL, MAVLINK_TYPE_INT64_T, 3, 119, offsetof(mavlink_test_types_t, s64_array) }, \
{ "f_array", NULL, MAVLINK_TYPE_FLOAT, 3, 143, offsetof(mavlink_test_types_t, f_array) }, \
{ "d_array", NULL, MAVLINK_TYPE_DOUBLE, 3, 155, offsetof(mavlink_test_types_t, d_array) }, \
} \
}
/**
* @brief Pack a test_types message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_test_types_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_char(buf, 0, c);
_mav_put_uint8_t(buf, 11, u8);
_mav_put_uint16_t(buf, 12, u16);
_mav_put_uint32_t(buf, 14, u32);
_mav_put_uint64_t(buf, 18, u64);
_mav_put_int8_t(buf, 26, s8);
_mav_put_int16_t(buf, 27, s16);
_mav_put_int32_t(buf, 29, s32);
_mav_put_int64_t(buf, 33, s64);
_mav_put_float(buf, 41, f);
_mav_put_double(buf, 45, d);
_mav_put_char_array(buf, 1, s, 10);
_mav_put_uint8_t_array(buf, 53, u8_array, 3);
_mav_put_uint16_t_array(buf, 56, u16_array, 3);
_mav_put_uint32_t_array(buf, 62, u32_array, 3);
_mav_put_uint64_t_array(buf, 74, u64_array, 3);
_mav_put_int8_t_array(buf, 98, s8_array, 3);
_mav_put_int16_t_array(buf, 101, s16_array, 3);
_mav_put_int32_t_array(buf, 107, s32_array, 3);
_mav_put_int64_t_array(buf, 119, s64_array, 3);
_mav_put_float_array(buf, 143, f_array, 3);
_mav_put_double_array(buf, 155, d_array, 3);
memcpy(_MAV_PAYLOAD(msg), buf, 179);
#else
mavlink_test_types_t packet;
packet.c = c;
packet.u8 = u8;
packet.u16 = u16;
packet.u32 = u32;
packet.u64 = u64;
packet.s8 = s8;
packet.s16 = s16;
packet.s32 = s32;
packet.s64 = s64;
packet.f = f;
packet.d = d;
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
memcpy(_MAV_PAYLOAD(msg), &packet, 179);
#endif
msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
return mavlink_finalize_message(msg, system_id, component_id, 179);
}
/**
* @brief Pack a test_types message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_char(buf, 0, c);
_mav_put_uint8_t(buf, 11, u8);
_mav_put_uint16_t(buf, 12, u16);
_mav_put_uint32_t(buf, 14, u32);
_mav_put_uint64_t(buf, 18, u64);
_mav_put_int8_t(buf, 26, s8);
_mav_put_int16_t(buf, 27, s16);
_mav_put_int32_t(buf, 29, s32);
_mav_put_int64_t(buf, 33, s64);
_mav_put_float(buf, 41, f);
_mav_put_double(buf, 45, d);
_mav_put_char_array(buf, 1, s, 10);
_mav_put_uint8_t_array(buf, 53, u8_array, 3);
_mav_put_uint16_t_array(buf, 56, u16_array, 3);
_mav_put_uint32_t_array(buf, 62, u32_array, 3);
_mav_put_uint64_t_array(buf, 74, u64_array, 3);
_mav_put_int8_t_array(buf, 98, s8_array, 3);
_mav_put_int16_t_array(buf, 101, s16_array, 3);
_mav_put_int32_t_array(buf, 107, s32_array, 3);
_mav_put_int64_t_array(buf, 119, s64_array, 3);
_mav_put_float_array(buf, 143, f_array, 3);
_mav_put_double_array(buf, 155, d_array, 3);
memcpy(_MAV_PAYLOAD(msg), buf, 179);
#else
mavlink_test_types_t packet;
packet.c = c;
packet.u8 = u8;
packet.u16 = u16;
packet.u32 = u32;
packet.u64 = u64;
packet.s8 = s8;
packet.s16 = s16;
packet.s32 = s32;
packet.s64 = s64;
packet.f = f;
packet.d = d;
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
memcpy(_MAV_PAYLOAD(msg), &packet, 179);
#endif
msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179);
}
/**
* @brief Encode a test_types struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param test_types C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types)
{
return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array);
}
/**
* @brief Send a test_types message
* @param chan MAVLink channel to send the message
*
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_char(buf, 0, c);
_mav_put_uint8_t(buf, 11, u8);
_mav_put_uint16_t(buf, 12, u16);
_mav_put_uint32_t(buf, 14, u32);
_mav_put_uint64_t(buf, 18, u64);
_mav_put_int8_t(buf, 26, s8);
_mav_put_int16_t(buf, 27, s16);
_mav_put_int32_t(buf, 29, s32);
_mav_put_int64_t(buf, 33, s64);
_mav_put_float(buf, 41, f);
_mav_put_double(buf, 45, d);
_mav_put_char_array(buf, 1, s, 10);
_mav_put_uint8_t_array(buf, 53, u8_array, 3);
_mav_put_uint16_t_array(buf, 56, u16_array, 3);
_mav_put_uint32_t_array(buf, 62, u32_array, 3);
_mav_put_uint64_t_array(buf, 74, u64_array, 3);
_mav_put_int8_t_array(buf, 98, s8_array, 3);
_mav_put_int16_t_array(buf, 101, s16_array, 3);
_mav_put_int32_t_array(buf, 107, s32_array, 3);
_mav_put_int64_t_array(buf, 119, s64_array, 3);
_mav_put_float_array(buf, 143, f_array, 3);
_mav_put_double_array(buf, 155, d_array, 3);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179);
#else
mavlink_test_types_t packet;
packet.c = c;
packet.u8 = u8;
packet.u16 = u16;
packet.u32 = u32;
packet.u64 = u64;
packet.s8 = s8;
packet.s16 = s16;
packet.s32 = s32;
packet.s64 = s64;
packet.f = f;
packet.d = d;
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179);
#endif
}
#endif
// MESSAGE TEST_TYPES UNPACKING
/**
* @brief Get field c from test_types message
*
* @return char
*/
static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg)
{
return _MAV_RETURN_char(msg, 0);
}
/**
* @brief Get field s from test_types message
*
* @return string
*/
static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s)
{
return _MAV_RETURN_char_array(msg, s, 10, 1);
}
/**
* @brief Get field u8 from test_types message
*
* @return uint8_t
*/
static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 11);
}
/**
* @brief Get field u16 from test_types message
*
* @return uint16_t
*/
static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 12);
}
/**
* @brief Get field u32 from test_types message
*
* @return uint32_t
*/
static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 14);
}
/**
* @brief Get field u64 from test_types message
*
* @return uint64_t
*/
static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint64_t(msg, 18);
}
/**
* @brief Get field s8 from test_types message
*
* @return int8_t
*/
static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg)
{
return _MAV_RETURN_int8_t(msg, 26);
}
/**
* @brief Get field s16 from test_types message
*
* @return int16_t
*/
static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 27);
}
/**
* @brief Get field s32 from test_types message
*
* @return int32_t
*/
static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 29);
}
/**
* @brief Get field s64 from test_types message
*
* @return int64_t
*/
static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg)
{
return _MAV_RETURN_int64_t(msg, 33);
}
/**
* @brief Get field f from test_types message
*
* @return float
*/
static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 41);
}
/**
* @brief Get field d from test_types message
*
* @return double
*/
static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg)
{
return _MAV_RETURN_double(msg, 45);
}
/**
* @brief Get field u8_array from test_types message
*
* @return uint8_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array)
{
return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 53);
}
/**
* @brief Get field u16_array from test_types message
*
* @return uint16_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array)
{
return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 56);
}
/**
* @brief Get field u32_array from test_types message
*
* @return uint32_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array)
{
return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 62);
}
/**
* @brief Get field u64_array from test_types message
*
* @return uint64_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array)
{
return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 74);
}
/**
* @brief Get field s8_array from test_types message
*
* @return int8_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array)
{
return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 98);
}
/**
* @brief Get field s16_array from test_types message
*
* @return int16_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array)
{
return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 101);
}
/**
* @brief Get field s32_array from test_types message
*
* @return int32_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array)
{
return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 107);
}
/**
* @brief Get field s64_array from test_types message
*
* @return int64_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array)
{
return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 119);
}
/**
* @brief Get field f_array from test_types message
*
* @return float_array
*/
static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array)
{
return _MAV_RETURN_float_array(msg, f_array, 3, 143);
}
/**
* @brief Get field d_array from test_types message
*
* @return double_array
*/
static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array)
{
return _MAV_RETURN_double_array(msg, d_array, 3, 155);
}
/**
* @brief Decode a test_types message into a struct
*
* @param msg The message to decode
* @param test_types C-struct to decode the message contents into
*/
static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types)
{
#if MAVLINK_NEED_BYTE_SWAP
test_types->c = mavlink_msg_test_types_get_c(msg);
mavlink_msg_test_types_get_s(msg, test_types->s);
test_types->u8 = mavlink_msg_test_types_get_u8(msg);
test_types->u16 = mavlink_msg_test_types_get_u16(msg);
test_types->u32 = mavlink_msg_test_types_get_u32(msg);
test_types->u64 = mavlink_msg_test_types_get_u64(msg);
test_types->s8 = mavlink_msg_test_types_get_s8(msg);
test_types->s16 = mavlink_msg_test_types_get_s16(msg);
test_types->s32 = mavlink_msg_test_types_get_s32(msg);
test_types->s64 = mavlink_msg_test_types_get_s64(msg);
test_types->f = mavlink_msg_test_types_get_f(msg);
test_types->d = mavlink_msg_test_types_get_d(msg);
mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array);
mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array);
mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array);
mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array);
mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array);
mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array);
mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array);
mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array);
mavlink_msg_test_types_get_f_array(msg, test_types->f_array);
mavlink_msg_test_types_get_d_array(msg, test_types->d_array);
#else
memcpy(test_types, _MAV_PAYLOAD(msg), 179);
#endif
}
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/test.h
-53
View File
@@ -1,53 +0,0 @@
/** @file
* @brief MAVLink comm protocol generated from test.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef TEST_H
#define TEST_H
#ifdef __cplusplus
extern "C" {
#endif
// MESSAGE LENGTHS AND CRCS
#ifndef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#endif
#ifndef MAVLINK_MESSAGE_INFO
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}}
#endif
#include "../protocol.h"
#define MAVLINK_ENABLED_TEST
// MAVLINK VERSION
#ifndef MAVLINK_VERSION
#define MAVLINK_VERSION 3
#endif
#if (MAVLINK_VERSION == 0)
#undef MAVLINK_VERSION
#define MAVLINK_VERSION 3
#endif
// ENUM DEFINITIONS
// MESSAGE DEFINITIONS
#include "./mavlink_msg_test_types.h"
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // TEST_H
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/testsuite.h
-120
View File
@@ -1,120 +0,0 @@
/** @file
* @brief MAVLink comm protocol testsuite generated from test.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef TEST_TESTSUITE_H
#define TEST_TESTSUITE_H
#ifdef __cplusplus
extern "C" {
#endif
#ifndef MAVLINK_TEST_ALL
#define MAVLINK_TEST_ALL
static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg);
static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_test(system_id, component_id, last_msg);
}
#endif
static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_test_types_t packet_in = {
'A',
"BCDEFGHIJ",
230,
17859,
963498192,
93372036854776941ULL,
211,
18639,
963498972,
93372036854777886LL,
304.0,
438.0,
{ 228, 229, 230 },
{ 20147, 20148, 20149 },
{ 963500688, 963500689, 963500690 },
{ 93372036854780469, 93372036854780470, 93372036854780471 },
{ 171, 172, 173 },
{ 22487, 22488, 22489 },
{ 963503028, 963503029, 963503030 },
{ 93372036854783304, 93372036854783305, 93372036854783306 },
{ 1018.0, 1019.0, 1020.0 },
{ 1208.0, 1209.0, 1210.0 },
};
mavlink_test_types_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.c = packet_in.c;
packet1.u8 = packet_in.u8;
packet1.u16 = packet_in.u16;
packet1.u32 = packet_in.u32;
packet1.u64 = packet_in.u64;
packet1.s8 = packet_in.s8;
packet1.s16 = packet_in.s16;
packet1.s32 = packet_in.s32;
packet1.s64 = packet_in.s64;
packet1.f = packet_in.f;
packet1.d = packet_in.d;
mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10);
mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3);
mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3);
mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_test_types_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_send(MAVLINK_COMM_1 , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_test(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_test_types(system_id, component_id, last_msg);
}
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // TEST_TESTSUITE_H
mavlink/share/pyshared/pymavlink/generator/C/include_v0.9/test/version.h
-12
View File
@@ -1,12 +0,0 @@
/** @file
* @brief MAVLink comm protocol built from test.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Thu Mar 1 15:11:54 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "0.9"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
#endif // MAVLINK_VERSION_H
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/checksum.h
-89
View File
@@ -1,89 +0,0 @@
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _CHECKSUM_H_
#define _CHECKSUM_H_
/**
*
* CALCULATE THE CHECKSUM
*
*/
#define X25_INIT_CRC 0xffff
#define X25_VALIDATE_CRC 0xf0b8
/**
* @brief Accumulate the X.25 CRC by adding one char at a time.
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new char to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
/*Accumulate one byte of data into the CRC*/
uint8_t tmp;
tmp = data ^ (uint8_t)(*crcAccum &0xff);
tmp ^= (tmp<<4);
*crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
}
/**
* @brief Initiliaze the buffer for the X.25 CRC
*
* @param crcAccum the 16 bit X.25 CRC
*/
static inline void crc_init(uint16_t* crcAccum)
{
*crcAccum = X25_INIT_CRC;
}
/**
* @brief Calculates the X.25 checksum on a byte buffer
*
* @param pBuffer buffer containing the byte array to hash
* @param length length of the byte array
* @return the checksum over the buffer bytes
**/
static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
{
uint16_t crcTmp;
crc_init(&crcTmp);
while (length--) {
crc_accumulate(*pBuffer++, &crcTmp);
}
return crcTmp;
}
/**
* @brief Accumulate the X.25 CRC by adding an array of bytes
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new bytes to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint8_t length)
{
const uint8_t *p = (const uint8_t *)pBuffer;
while (length--) {
crc_accumulate(*p++, crcAccum);
}
}
#endif /* _CHECKSUM_H_ */
#ifdef __cplusplus
}
#endif
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_helpers.h
-507
View File
@@ -1,507 +0,0 @@
#ifndef _MAVLINK_HELPERS_H_
#define _MAVLINK_HELPERS_H_
#include "string.h"
#include "checksum.h"
#include "mavlink_types.h"
#ifndef MAVLINK_HELPER
#define MAVLINK_HELPER
#endif
/*
internal function to give access to the channel status for each channel
*/
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan)
{
static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
return &m_mavlink_status[chan];
}
/*
internal function to give access to the channel buffer for each channel
*/
MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
{
#if MAVLINK_EXTERNAL_RX_BUFFER
// No m_mavlink_message array defined in function,
// has to be defined externally
#ifndef m_mavlink_message
#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION (mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];)
#endif
#else
static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
#endif
return &m_mavlink_buffer[chan];
}
/**
* @brief Finalize a MAVLink message with channel assignment
*
* This function calculates the checksum and sets length and aircraft id correctly.
* It assumes that the message id and the payload are already correctly set. This function
* can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
* instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
*
* @param msg Message to finalize
* @param system_id Id of the sending (this) system, 1-127
* @param length Message length
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length)
#endif
{
// This code part is the same for all messages;
uint16_t checksum;
msg->magic = MAVLINK_STX;
msg->len = length;
msg->sysid = system_id;
msg->compid = component_id;
// One sequence number per component
msg->seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1;
checksum = crc_calculate((uint8_t*)&msg->len, length + MAVLINK_CORE_HEADER_LEN);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
#endif
mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF);
mavlink_ck_b(msg) = (uint8_t)(checksum >> 8);
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
/**
* @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length, uint8_t crc_extra)
{
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
}
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length)
{
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
/**
* @brief Finalize a MAVLink message with channel assignment and send
*/
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
uint8_t length, uint8_t crc_extra)
#else
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
#endif
{
uint16_t checksum;
uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
uint8_t ck[2];
mavlink_status_t *status = mavlink_get_channel_status(chan);
buf[0] = MAVLINK_STX;
buf[1] = length;
buf[2] = status->current_tx_seq;
buf[3] = mavlink_system.sysid;
buf[4] = mavlink_system.compid;
buf[5] = msgid;
status->current_tx_seq++;
checksum = crc_calculate((uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, packet, length);
#if MAVLINK_CRC_EXTRA
crc_accumulate(crc_extra, &checksum);
#endif
ck[0] = (uint8_t)(checksum & 0xFF);
ck[1] = (uint8_t)(checksum >> 8);
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
_mavlink_send_uart(chan, packet, length);
_mavlink_send_uart(chan, (const char *)ck, 2);
MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
}
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
/**
* @brief Pack a message to send it over a serial byte stream
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}
union __mavlink_bitfield {
uint8_t uint8;
int8_t int8;
uint16_t uint16;
int16_t int16;
uint32_t uint32;
int32_t int32;
};
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg)
{
crc_init(&msg->checksum);
}
MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
{
crc_accumulate(c, &msg->checksum);
}
/**
* This is a convenience function which handles the complete MAVLink parsing.
* the function will parse one byte at a time and return the complete packet once
* it could be successfully decoded. Checksum and other failures will be silently
* ignored.
*
* @param chan ID of the current channel. This allows to parse different channels with this function.
* a channel is not a physical message channel like a serial port, but a logic partition of
* the communication streams in this case. COMM_NB is the limit for the number of channels
* on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
* @param c The char to barse
*
* @param returnMsg NULL if no message could be decoded, the message data else
* @return 0 if no message could be decoded, 1 else
*
* A typical use scenario of this function call is:
*
* @code
* #include <inttypes.h> // For fixed-width uint8_t type
*
* mavlink_message_t msg;
* int chan = 0;
*
*
* while(serial.bytesAvailable > 0)
* {
* uint8_t byte = serial.getNextByte();
* if (mavlink_parse_char(chan, byte, &msg))
* {
* printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
* }
* }
*
*
* @endcode
*/
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
/*
default message crc function. You can override this per-system to
put this data in a different memory segment
*/
#if MAVLINK_CRC_EXTRA
#ifndef MAVLINK_MESSAGE_CRC
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
#endif
#endif
mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); ///< The currently decoded message
mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status
int bufferIndex = 0;
status->msg_received = 0;
switch (status->parse_state)
{
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
rxmsg->magic = c;
mavlink_start_checksum(rxmsg);
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
if (status->msg_received
/* Support shorter buffers than the
default maximum packet size */
#if (MAVLINK_MAX_PAYLOAD_LEN < 255)
|| c > MAVLINK_MAX_PAYLOAD_LEN
#endif
)
{
status->buffer_overrun++;
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
}
else
{
// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
rxmsg->len = c;
status->packet_idx = 0;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
}
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
rxmsg->seq = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
rxmsg->sysid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
rxmsg->compid = c;
mavlink_update_checksum(rxmsg, c);
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
rxmsg->msgid = c;
mavlink_update_checksum(rxmsg, c);
if (rxmsg->len == 0)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
else
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID:
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
mavlink_update_checksum(rxmsg, c);
if (status->packet_idx == rxmsg->len)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
#if MAVLINK_CRC_EXTRA
mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
#endif
if (c != (rxmsg->checksum & 0xFF)) {
// Check first checksum byte
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
}
else
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC1:
if (c != (rxmsg->checksum >> 8)) {
// Check second checksum byte
status->parse_error++;
status->msg_received = 0;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX)
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
mavlink_start_checksum(rxmsg);
}
}
else
{
// Successfully got message
status->msg_received = 1;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
break;
}
bufferIndex++;
// If a message has been sucessfully decoded, check index
if (status->msg_received == 1)
{
//while(status->current_seq != rxmsg->seq)
//{
// status->packet_rx_drop_count++;
// status->current_seq++;
//}
status->current_rx_seq = rxmsg->seq;
// Initial condition: If no packet has been received so far, drop count is undefined
if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0;
// Count this packet as received
status->packet_rx_success_count++;
}
r_mavlink_status->current_rx_seq = status->current_rx_seq+1;
r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count;
r_mavlink_status->packet_rx_drop_count = status->parse_error;
status->parse_error = 0;
return status->msg_received;
}
/**
* @brief Put a bitfield of length 1-32 bit into the buffer
*
* @param b the value to add, will be encoded in the bitfield
* @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc.
* @param packet_index the position in the packet (the index of the first byte to use)
* @param bit_index the position in the byte (the index of the first bit to use)
* @param buffer packet buffer to write into
* @return new position of the last used byte in the buffer
*/
MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer)
{
uint16_t bits_remain = bits;
// Transform number into network order
int32_t v;
uint8_t i_bit_index, i_byte_index, curr_bits_n;
#if MAVLINK_NEED_BYTE_SWAP
union {
int32_t i;
uint8_t b[4];
} bin, bout;
bin.i = b;
bout.b[0] = bin.b[3];
bout.b[1] = bin.b[2];
bout.b[2] = bin.b[1];
bout.b[3] = bin.b[0];
v = bout.i;
#else
v = b;
#endif
// buffer in
// 01100000 01000000 00000000 11110001
// buffer out
// 11110001 00000000 01000000 01100000
// Existing partly filled byte (four free slots)
// 0111xxxx
// Mask n free bits
// 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1
// = ((uint32_t)(1 << n)) - 1; // = 2^n - 1
// Shift n bits into the right position
// out = in >> n;
// Mask and shift bytes
i_bit_index = bit_index;
i_byte_index = packet_index;
if (bit_index > 0)
{
// If bits were available at start, they were available
// in the byte before the current index
i_byte_index--;
}
// While bits have not been packed yet
while (bits_remain > 0)
{
// Bits still have to be packed
// there can be more than 8 bits, so
// we might have to pack them into more than one byte
// First pack everything we can into the current 'open' byte
//curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
//FIXME
if (bits_remain <= (uint8_t)(8 - i_bit_index))
{
// Enough space
curr_bits_n = (uint8_t)bits_remain;
}
else
{
curr_bits_n = (8 - i_bit_index);
}
// Pack these n bits into the current byte
// Mask out whatever was at that position with ones (xxx11111)
buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n));
// Put content to this position, by masking out the non-used part
buffer[i_byte_index] |= ((0x00 << curr_bits_n) & v);
// Increment the bit index
i_bit_index += curr_bits_n;
// Now proceed to the next byte, if necessary
bits_remain -= curr_bits_n;
if (bits_remain > 0)
{
// Offer another 8 bits / one byte
i_byte_index++;
i_bit_index = 0;
}
}
*r_bit_index = i_bit_index;
// If a partly filled byte is present, mark this as consumed
if (i_bit_index != 7) i_byte_index++;
return i_byte_index - packet_index;
}
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
// To make MAVLink work on your MCU, define comm_send_ch() if you wish
// to send 1 byte at a time, or MAVLINK_SEND_UART_BYTES() to send a
// whole packet at a time
/*
#include "mavlink_types.h"
void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
if (chan == MAVLINK_COMM_0)
{
uart0_transmit(ch);
}
if (chan == MAVLINK_COMM_1)
{
uart1_transmit(ch);
}
}
*/
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len)
{
#ifdef MAVLINK_SEND_UART_BYTES
/* this is the more efficient approach, if the platform
defines it */
MAVLINK_SEND_UART_BYTES(chan, (uint8_t *)buf, len);
#else
/* fallback to one byte at a time */
uint16_t i;
for (i = 0; i < len; i++) {
comm_send_ch(chan, (uint8_t)buf[i]);
}
#endif
}
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
#endif /* _MAVLINK_HELPERS_H_ */
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_protobuf_manager.hpp
-377
View File
@@ -1,377 +0,0 @@
#ifndef MAVLINKPROTOBUFMANAGER_HPP
#define MAVLINKPROTOBUFMANAGER_HPP
#include <deque>
#include <google/protobuf/message.h>
#include <iostream>
#include <tr1/memory>
#include <checksum.h>
#include <common/mavlink.h>
#include <mavlink_types.h>
#include <pixhawk/pixhawk.pb.h>
namespace mavlink
{
class ProtobufManager
{
public:
ProtobufManager()
: mRegisteredTypeCount(0)
, mStreamID(0)
, mVerbose(false)
, kExtendedHeaderSize(MAVLINK_EXTENDED_HEADER_LEN)
, kExtendedPayloadMaxSize(MAVLINK_MAX_EXTENDED_PAYLOAD_LEN)
{
// register GLOverlay
{
std::tr1::shared_ptr<px::GLOverlay> msg(new px::GLOverlay);
registerType(msg);
}
// register ObstacleList
{
std::tr1::shared_ptr<px::ObstacleList> msg(new px::ObstacleList);
registerType(msg);
}
// register ObstacleMap
{
std::tr1::shared_ptr<px::ObstacleMap> msg(new px::ObstacleMap);
registerType(msg);
}
// register Path
{
std::tr1::shared_ptr<px::Path> msg(new px::Path);
registerType(msg);
}
// register PointCloudXYZI
{
std::tr1::shared_ptr<px::PointCloudXYZI> msg(new px::PointCloudXYZI);
registerType(msg);
}
// register PointCloudXYZRGB
{
std::tr1::shared_ptr<px::PointCloudXYZRGB> msg(new px::PointCloudXYZRGB);
registerType(msg);
}
// register RGBDImage
{
std::tr1::shared_ptr<px::RGBDImage> msg(new px::RGBDImage);
registerType(msg);
}
srand(time(NULL));
mStreamID = rand() + 1;
}
bool fragmentMessage(uint8_t system_id, uint8_t component_id,
uint8_t target_system, uint8_t target_component,
const google::protobuf::Message& protobuf_msg,
std::vector<mavlink_extended_message_t>& fragments) const
{
TypeMap::const_iterator it = mTypeMap.find(protobuf_msg.GetTypeName());
if (it == mTypeMap.end())
{
std::cout << "# WARNING: Protobuf message with type "
<< protobuf_msg.GetTypeName() << " is not registered."
<< std::endl;
return false;
}
uint8_t typecode = it->second;
std::string data = protobuf_msg.SerializeAsString();
int fragmentCount = (protobuf_msg.ByteSize() + kExtendedPayloadMaxSize - 1) / kExtendedPayloadMaxSize;
unsigned int offset = 0;
for (int i = 0; i < fragmentCount; ++i)
{
mavlink_extended_message_t fragment;
// write extended header data
uint8_t* payload = reinterpret_cast<uint8_t*>(fragment.base_msg.payload64);
unsigned int length = 0;
uint8_t flags = 0;
if (i < fragmentCount - 1)
{
length = kExtendedPayloadMaxSize;
flags |= 0x1;
}
else
{
length = protobuf_msg.ByteSize() - kExtendedPayloadMaxSize * (fragmentCount - 1);
}
memcpy(payload, &target_system, 1);
memcpy(payload + 1, &target_component, 1);
memcpy(payload + 2, &typecode, 1);
memcpy(payload + 3, &length, 4);
memcpy(payload + 7, &mStreamID, 2);
memcpy(payload + 9, &offset, 4);
memcpy(payload + 13, &flags, 1);
fragment.base_msg.msgid = MAVLINK_MSG_ID_EXTENDED_MESSAGE;
mavlink_finalize_message(&fragment.base_msg, system_id, component_id, kExtendedHeaderSize, 0);
// write extended payload data
fragment.extended_payload_len = length;
memcpy(fragment.extended_payload, &data[offset], length);
fragments.push_back(fragment);
offset += length;
}
if (mVerbose)
{
std::cerr << "# INFO: Split extended message with size "
<< protobuf_msg.ByteSize() << " into "
<< fragmentCount << " fragments." << std::endl;
}
return true;
}
bool cacheFragment(mavlink_extended_message_t& msg)
{
if (!validFragment(msg))
{
if (mVerbose)
{
std::cerr << "# WARNING: Message is not a valid fragment. "
<< "Dropping message..." << std::endl;
}
return false;
}
// read extended header
uint8_t* payload = reinterpret_cast<uint8_t*>(msg.base_msg.payload64);
uint8_t typecode = 0;
unsigned int length = 0;
unsigned short streamID = 0;
unsigned int offset = 0;
uint8_t flags = 0;
memcpy(&typecode, payload + 2, 1);
memcpy(&length, payload + 3, 4);
memcpy(&streamID, payload + 7, 2);
memcpy(&offset, payload + 9, 4);
memcpy(&flags, payload + 13, 1);
if (typecode >= mTypeMap.size())
{
std::cout << "# WARNING: Protobuf message with type code "
<< static_cast<int>(typecode) << " is not registered." << std::endl;
return false;
}
bool reassemble = false;
FragmentQueue::iterator it = mFragmentQueue.find(streamID);
if (it == mFragmentQueue.end())
{
if (offset == 0)
{
mFragmentQueue[streamID].push_back(msg);
if ((flags & 0x1) != 0x1)
{
reassemble = true;
}
if (mVerbose)
{
std::cerr << "# INFO: Added fragment to new queue."
<< std::endl;
}
}
else
{
if (mVerbose)
{
std::cerr << "# WARNING: Message is not a valid fragment. "
<< "Dropping message..." << std::endl;
}
}
}
else
{
std::deque<mavlink_extended_message_t>& queue = it->second;
if (queue.empty())
{
if (offset == 0)
{
queue.push_back(msg);
if ((flags & 0x1) != 0x1)
{
reassemble = true;
}
}
else
{
if (mVerbose)
{
std::cerr << "# WARNING: Message is not a valid fragment. "
<< "Dropping message..." << std::endl;
}
}
}
else
{
if (fragmentDataSize(queue.back()) + fragmentOffset(queue.back()) != offset)
{
if (mVerbose)
{
std::cerr << "# WARNING: Previous fragment(s) have been lost. "
<< "Dropping message and clearing queue..." << std::endl;
}
queue.clear();
}
else
{
queue.push_back(msg);
if ((flags & 0x1) != 0x1)
{
reassemble = true;
}
}
}
}
if (reassemble)
{
std::deque<mavlink_extended_message_t>& queue = mFragmentQueue[streamID];
std::string data;
for (size_t i = 0; i < queue.size(); ++i)
{
mavlink_extended_message_t& mavlink_msg = queue.at(i);
data.append(reinterpret_cast<char*>(&mavlink_msg.extended_payload[0]),
static_cast<size_t>(mavlink_msg.extended_payload_len));
}
mMessages.at(typecode)->ParseFromString(data);
mMessageAvailable.at(typecode) = true;
queue.clear();
if (mVerbose)
{
std::cerr << "# INFO: Reassembled fragments for message with typename "
<< mMessages.at(typecode)->GetTypeName() << " and size "
<< mMessages.at(typecode)->ByteSize()
<< "." << std::endl;
}
}
return true;
}
bool getMessage(std::tr1::shared_ptr<google::protobuf::Message>& msg)
{
for (size_t i = 0; i < mMessageAvailable.size(); ++i)
{
if (mMessageAvailable.at(i))
{
msg = mMessages.at(i);
mMessageAvailable.at(i) = false;
return true;
}
}
return false;
}
private:
void registerType(const std::tr1::shared_ptr<google::protobuf::Message>& msg)
{
mTypeMap[msg->GetTypeName()] = mRegisteredTypeCount;
++mRegisteredTypeCount;
mMessages.push_back(msg);
mMessageAvailable.push_back(false);
}
bool validFragment(const mavlink_extended_message_t& msg) const
{
if (msg.base_msg.magic != MAVLINK_STX ||
msg.base_msg.len != kExtendedHeaderSize ||
msg.base_msg.msgid != MAVLINK_MSG_ID_EXTENDED_MESSAGE)
{
return false;
}
uint16_t checksum;
checksum = crc_calculate(reinterpret_cast<const uint8_t*>(&msg.base_msg.len), MAVLINK_CORE_HEADER_LEN);
crc_accumulate_buffer(&checksum, reinterpret_cast<const char*>(&msg.base_msg.payload64), kExtendedHeaderSize);
#if MAVLINK_CRC_EXTRA
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
crc_accumulate(mavlink_message_crcs[msg.base_msg.msgid], &checksum);
#endif
if (mavlink_ck_a(&(msg.base_msg)) != (uint8_t)(checksum & 0xFF) &&
mavlink_ck_b(&(msg.base_msg)) != (uint8_t)(checksum >> 8))
{
return false;
}
return true;
}
unsigned int fragmentDataSize(const mavlink_extended_message_t& msg) const
{
const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
return *(reinterpret_cast<const unsigned int*>(payload + 3));
}
unsigned int fragmentOffset(const mavlink_extended_message_t& msg) const
{
const uint8_t* payload = reinterpret_cast<const uint8_t*>(msg.base_msg.payload64);
return *(reinterpret_cast<const unsigned int*>(payload + 9));
}
int mRegisteredTypeCount;
unsigned short mStreamID;
bool mVerbose;
typedef std::map<std::string, uint8_t> TypeMap;
TypeMap mTypeMap;
std::vector< std::tr1::shared_ptr<google::protobuf::Message> > mMessages;
std::vector<bool> mMessageAvailable;
typedef std::map<unsigned short, std::deque<mavlink_extended_message_t> > FragmentQueue;
FragmentQueue mFragmentQueue;
const int kExtendedHeaderSize;
/**
* Extended header structure
* =========================
* byte 0 - target_system
* byte 1 - target_component
* byte 2 - extended message id (type code)
* bytes 3-6 - extended payload size in bytes
* byte 7-8 - stream ID
* byte 9-12 - fragment offset
* byte 13 - fragment flags (bit 0 - 1=more fragments, 0=last fragment)
*/
const int kExtendedPayloadMaxSize;
};
}
#endif
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/mavlink_types.h
-158
View File
@@ -1,158 +0,0 @@
#ifndef MAVLINK_TYPES_H_
#define MAVLINK_TYPES_H_
#include <inttypes.h>
#ifndef MAVLINK_MAX_PAYLOAD_LEN
// it is possible to override this, but be careful!
#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
#endif
#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
#define MAVLINK_NUM_CHECKSUM_BYTES 2
#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
#define MAVLINK_EXTENDED_HEADER_LEN 14
#if (defined _MSC_VER) | ((defined __APPLE__) & (defined __MACH__)) | (defined __linux__)
/* full fledged 32bit++ OS */
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
#else
/* small microcontrollers */
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
#endif
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
typedef struct param_union {
union {
float param_float;
int32_t param_int32;
uint32_t param_uint32;
uint8_t param_uint8;
uint8_t bytes[4];
};
uint8_t type;
} mavlink_param_union_t;
typedef struct __mavlink_system {
uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function
uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function
uint8_t type; ///< Unused, can be used by user to store the system's type
uint8_t state; ///< Unused, can be used by user to store the system's state
uint8_t mode; ///< Unused, can be used by user to store the system's mode
uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
} mavlink_system_t;
typedef struct __mavlink_message {
uint16_t checksum; /// sent at end of packet
uint8_t magic; ///< protocol magic marker
uint8_t len; ///< Length of payload
uint8_t seq; ///< Sequence of packet
uint8_t sysid; ///< ID of message sender system/aircraft
uint8_t compid; ///< ID of the message sender component
uint8_t msgid; ///< ID of message in payload
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
} mavlink_message_t;
typedef struct __mavlink_extended_message {
mavlink_message_t base_msg;
int32_t extended_payload_len; ///< Length of extended payload if any
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
} mavlink_extended_message_t;
typedef enum {
MAVLINK_TYPE_CHAR = 0,
MAVLINK_TYPE_UINT8_T = 1,
MAVLINK_TYPE_INT8_T = 2,
MAVLINK_TYPE_UINT16_T = 3,
MAVLINK_TYPE_INT16_T = 4,
MAVLINK_TYPE_UINT32_T = 5,
MAVLINK_TYPE_INT32_T = 6,
MAVLINK_TYPE_UINT64_T = 7,
MAVLINK_TYPE_INT64_T = 8,
MAVLINK_TYPE_FLOAT = 9,
MAVLINK_TYPE_DOUBLE = 10
} mavlink_message_type_t;
#define MAVLINK_MAX_FIELDS 64
typedef struct __mavlink_field_info {
const char *name; // name of this field
const char *print_format; // printing format hint, or NULL
mavlink_message_type_t type; // type of this field
unsigned int array_length; // if non-zero, field is an array
unsigned int wire_offset; // offset of each field in the payload
unsigned int structure_offset; // offset in a C structure
} mavlink_field_info_t;
// note that in this structure the order of fields is the order
// in the XML file, not necessary the wire order
typedef struct __mavlink_message_info {
const char *name; // name of the message
unsigned num_fields; // how many fields in this message
mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
} mavlink_message_info_t;
#define _MAV_PAYLOAD(msg) ((const char *)(&((msg)->payload64[0])))
#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload64[0])))
// checksum is immediately after the payload bytes
#define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
#define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
typedef enum {
MAVLINK_COMM_0,
MAVLINK_COMM_1,
MAVLINK_COMM_2,
MAVLINK_COMM_3
} mavlink_channel_t;
/*
* applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number
* of buffers they will use. If more are used, then the result will be
* a stack overrun
*/
#ifndef MAVLINK_COMM_NUM_BUFFERS
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32)
# define MAVLINK_COMM_NUM_BUFFERS 16
#else
# define MAVLINK_COMM_NUM_BUFFERS 4
#endif
#endif
typedef enum {
MAVLINK_PARSE_STATE_UNINIT=0,
MAVLINK_PARSE_STATE_IDLE,
MAVLINK_PARSE_STATE_GOT_STX,
MAVLINK_PARSE_STATE_GOT_SEQ,
MAVLINK_PARSE_STATE_GOT_LENGTH,
MAVLINK_PARSE_STATE_GOT_SYSID,
MAVLINK_PARSE_STATE_GOT_COMPID,
MAVLINK_PARSE_STATE_GOT_MSGID,
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
MAVLINK_PARSE_STATE_GOT_CRC1
} mavlink_parse_state_t; ///< The state machine for the comm parser
typedef struct __mavlink_status {
uint8_t msg_received; ///< Number of received messages
uint8_t buffer_overrun; ///< Number of buffer overruns
uint8_t parse_error; ///< Number of parse errors
mavlink_parse_state_t parse_state; ///< Parsing state machine
uint8_t packet_idx; ///< Index in current packet
uint8_t current_rx_seq; ///< Sequence number of last packet received
uint8_t current_tx_seq; ///< Sequence number of last packet sent
uint16_t packet_rx_success_count; ///< Received packets
uint16_t packet_rx_drop_count; ///< Number of packet drops
} mavlink_status_t;
#define MAVLINK_BIG_ENDIAN 0
#define MAVLINK_LITTLE_ENDIAN 1
#endif /* MAVLINK_TYPES_H_ */
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/pixhawk/pixhawk.pb.h
-3663
View File
File diff suppressed because it is too large Load Diff
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/protocol.h
-322
View File
@@ -1,322 +0,0 @@
#ifndef _MAVLINK_PROTOCOL_H_
#define _MAVLINK_PROTOCOL_H_
#include "string.h"
#include "mavlink_types.h"
/*
If you want MAVLink on a system that is native big-endian,
you need to define NATIVE_BIG_ENDIAN
*/
#ifdef NATIVE_BIG_ENDIAN
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN == MAVLINK_LITTLE_ENDIAN)
#else
# define MAVLINK_NEED_BYTE_SWAP (MAVLINK_ENDIAN != MAVLINK_LITTLE_ENDIAN)
#endif
#ifndef MAVLINK_STACK_BUFFER
#define MAVLINK_STACK_BUFFER 0
#endif
#ifndef MAVLINK_AVOID_GCC_STACK_BUG
# define MAVLINK_AVOID_GCC_STACK_BUG defined(__GNUC__)
#endif
#ifndef MAVLINK_ASSERT
#define MAVLINK_ASSERT(x)
#endif
#ifndef MAVLINK_START_UART_SEND
#define MAVLINK_START_UART_SEND(chan, length)
#endif
#ifndef MAVLINK_END_UART_SEND
#define MAVLINK_END_UART_SEND(chan, length)
#endif
#ifdef MAVLINK_SEPARATE_HELPERS
#define MAVLINK_HELPER
#else
#define MAVLINK_HELPER static inline
#include "mavlink_helpers.h"
#endif // MAVLINK_SEPARATE_HELPERS
/* always include the prototypes to ensure we don't get out of sync */
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
#if MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length, uint8_t crc_extra);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length, uint8_t crc_extra);
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
uint8_t length, uint8_t crc_extra);
#endif
#else
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t chan, uint8_t length);
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
uint8_t length);
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
#endif // MAVLINK_CRC_EXTRA
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
uint8_t* r_bit_index, uint8_t* buffer);
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
#endif
/**
* @brief Get the required buffer size for this message
*/
static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
{
return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
#if MAVLINK_NEED_BYTE_SWAP
static inline void byte_swap_2(char *dst, const char *src)
{
dst[0] = src[1];
dst[1] = src[0];
}
static inline void byte_swap_4(char *dst, const char *src)
{
dst[0] = src[3];
dst[1] = src[2];
dst[2] = src[1];
dst[3] = src[0];
}
static inline void byte_swap_8(char *dst, const char *src)
{
dst[0] = src[7];
dst[1] = src[6];
dst[2] = src[5];
dst[3] = src[4];
dst[4] = src[3];
dst[5] = src[2];
dst[6] = src[1];
dst[7] = src[0];
}
#elif !MAVLINK_ALIGNED_FIELDS
static inline void byte_copy_2(char *dst, const char *src)
{
dst[0] = src[0];
dst[1] = src[1];
}
static inline void byte_copy_4(char *dst, const char *src)
{
dst[0] = src[0];
dst[1] = src[1];
dst[2] = src[2];
dst[3] = src[3];
}
static inline void byte_copy_8(char *dst, const char *src)
{
memcpy(dst, src, 8);
}
#endif
#define _mav_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
#define _mav_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b
#define _mav_put_char(buf, wire_offset, b) buf[wire_offset] = b
#if MAVLINK_NEED_BYTE_SWAP
#define _mav_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
#elif !MAVLINK_ALIGNED_FIELDS
#define _mav_put_uint16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_int16_t(buf, wire_offset, b) byte_copy_2(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_int32_t(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_uint64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_int64_t(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#define _mav_put_float(buf, wire_offset, b) byte_copy_4(&buf[wire_offset], (const char *)&b)
#define _mav_put_double(buf, wire_offset, b) byte_copy_8(&buf[wire_offset], (const char *)&b)
#else
#define _mav_put_uint16_t(buf, wire_offset, b) *(uint16_t *)&buf[wire_offset] = b
#define _mav_put_int16_t(buf, wire_offset, b) *(int16_t *)&buf[wire_offset] = b
#define _mav_put_uint32_t(buf, wire_offset, b) *(uint32_t *)&buf[wire_offset] = b
#define _mav_put_int32_t(buf, wire_offset, b) *(int32_t *)&buf[wire_offset] = b
#define _mav_put_uint64_t(buf, wire_offset, b) *(uint64_t *)&buf[wire_offset] = b
#define _mav_put_int64_t(buf, wire_offset, b) *(int64_t *)&buf[wire_offset] = b
#define _mav_put_float(buf, wire_offset, b) *(float *)&buf[wire_offset] = b
#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b
#endif
/*
like memcpy(), but if src is NULL, do a memset to zero
*/
static void mav_array_memcpy(void *dest, const void *src, size_t n)
{
if (src == NULL) {
memset(dest, 0, n);
} else {
memcpy(dest, src, n);
}
}
/*
* Place a char array into a buffer
*/
static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
* Place a uint8_t array into a buffer
*/
static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
* Place a int8_t array into a buffer
*/
static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
{
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_PUT_ARRAY(TYPE, V) \
static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
{ \
if (b == NULL) { \
memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
} else { \
uint16_t i; \
for (i=0; i<array_length; i++) { \
_mav_put_## TYPE (buf, wire_offset+(i*sizeof(TYPE)), b[i]); \
} \
} \
}
#else
#define _MAV_PUT_ARRAY(TYPE, V) \
static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
{ \
mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
}
#endif
_MAV_PUT_ARRAY(uint16_t, u16)
_MAV_PUT_ARRAY(uint32_t, u32)
_MAV_PUT_ARRAY(uint64_t, u64)
_MAV_PUT_ARRAY(int16_t, i16)
_MAV_PUT_ARRAY(int32_t, i32)
_MAV_PUT_ARRAY(int64_t, i64)
_MAV_PUT_ARRAY(float, f)
_MAV_PUT_ARRAY(double, d)
#define _MAV_RETURN_char(msg, wire_offset) (const char)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset) (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#elif !MAVLINK_ALIGNED_FIELDS
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; }
_MAV_MSG_RETURN_TYPE(uint16_t, 2)
_MAV_MSG_RETURN_TYPE(int16_t, 2)
_MAV_MSG_RETURN_TYPE(uint32_t, 4)
_MAV_MSG_RETURN_TYPE(int32_t, 4)
_MAV_MSG_RETURN_TYPE(uint64_t, 8)
_MAV_MSG_RETURN_TYPE(int64_t, 8)
_MAV_MSG_RETURN_TYPE(float, 4)
_MAV_MSG_RETURN_TYPE(double, 8)
#else // nicely aligned, no swap
#define _MAV_MSG_RETURN_TYPE(TYPE) \
static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \
{ return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);}
_MAV_MSG_RETURN_TYPE(uint16_t)
_MAV_MSG_RETURN_TYPE(int16_t)
_MAV_MSG_RETURN_TYPE(uint32_t)
_MAV_MSG_RETURN_TYPE(int32_t)
_MAV_MSG_RETURN_TYPE(uint64_t)
_MAV_MSG_RETURN_TYPE(int64_t)
_MAV_MSG_RETURN_TYPE(float)
_MAV_MSG_RETURN_TYPE(double)
#endif // MAVLINK_NEED_BYTE_SWAP
static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value,
uint8_t array_length, uint8_t wire_offset)
{
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length);
return array_length;
}
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
uint16_t i; \
for (i=0; i<array_length; i++) { \
value[i] = _MAV_RETURN_## TYPE (msg, wire_offset+(i*sizeof(value[0]))); \
} \
return array_length*sizeof(value[0]); \
}
#else
#define _MAV_RETURN_ARRAY(TYPE, V) \
static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \
uint8_t array_length, uint8_t wire_offset) \
{ \
memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \
return array_length*sizeof(TYPE); \
}
#endif
_MAV_RETURN_ARRAY(uint16_t, u16)
_MAV_RETURN_ARRAY(uint32_t, u32)
_MAV_RETURN_ARRAY(uint64_t, u64)
_MAV_RETURN_ARRAY(int16_t, i16)
_MAV_RETURN_ARRAY(int32_t, i32)
_MAV_RETURN_ARRAY(int64_t, i64)
_MAV_RETURN_ARRAY(float, f)
_MAV_RETURN_ARRAY(double, d)
#endif // _MAVLINK_PROTOCOL_H_
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink.h
-27
View File
@@ -1,27 +0,0 @@
/** @file
* @brief MAVLink comm protocol built from test.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#ifndef MAVLINK_STX
#define MAVLINK_STX 254
#endif
#ifndef MAVLINK_ENDIAN
#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
#endif
#ifndef MAVLINK_ALIGNED_FIELDS
#define MAVLINK_ALIGNED_FIELDS 1
#endif
#ifndef MAVLINK_CRC_EXTRA
#define MAVLINK_CRC_EXTRA 1
#endif
#include "version.h"
#include "test.h"
#endif // MAVLINK_H
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/mavlink_msg_test_types.h
-610
View File
@@ -1,610 +0,0 @@
// MESSAGE TEST_TYPES PACKING
#define MAVLINK_MSG_ID_TEST_TYPES 0
typedef struct __mavlink_test_types_t
{
uint64_t u64; ///< uint64_t
int64_t s64; ///< int64_t
double d; ///< double
uint64_t u64_array[3]; ///< uint64_t_array
int64_t s64_array[3]; ///< int64_t_array
double d_array[3]; ///< double_array
uint32_t u32; ///< uint32_t
int32_t s32; ///< int32_t
float f; ///< float
uint32_t u32_array[3]; ///< uint32_t_array
int32_t s32_array[3]; ///< int32_t_array
float f_array[3]; ///< float_array
uint16_t u16; ///< uint16_t
int16_t s16; ///< int16_t
uint16_t u16_array[3]; ///< uint16_t_array
int16_t s16_array[3]; ///< int16_t_array
char c; ///< char
char s[10]; ///< string
uint8_t u8; ///< uint8_t
int8_t s8; ///< int8_t
uint8_t u8_array[3]; ///< uint8_t_array
int8_t s8_array[3]; ///< int8_t_array
} mavlink_test_types_t;
#define MAVLINK_MSG_ID_TEST_TYPES_LEN 179
#define MAVLINK_MSG_ID_0_LEN 179
#define MAVLINK_MSG_TEST_TYPES_FIELD_U64_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S64_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_D_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_U32_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S32_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_F_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_U16_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S16_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S_LEN 10
#define MAVLINK_MSG_TEST_TYPES_FIELD_U8_ARRAY_LEN 3
#define MAVLINK_MSG_TEST_TYPES_FIELD_S8_ARRAY_LEN 3
#define MAVLINK_MESSAGE_INFO_TEST_TYPES { \
"TEST_TYPES", \
22, \
{ { "u64", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_test_types_t, u64) }, \
{ "s64", NULL, MAVLINK_TYPE_INT64_T, 0, 8, offsetof(mavlink_test_types_t, s64) }, \
{ "d", NULL, MAVLINK_TYPE_DOUBLE, 0, 16, offsetof(mavlink_test_types_t, d) }, \
{ "u64_array", NULL, MAVLINK_TYPE_UINT64_T, 3, 24, offsetof(mavlink_test_types_t, u64_array) }, \
{ "s64_array", NULL, MAVLINK_TYPE_INT64_T, 3, 48, offsetof(mavlink_test_types_t, s64_array) }, \
{ "d_array", NULL, MAVLINK_TYPE_DOUBLE, 3, 72, offsetof(mavlink_test_types_t, d_array) }, \
{ "u32", "0x%08x", MAVLINK_TYPE_UINT32_T, 0, 96, offsetof(mavlink_test_types_t, u32) }, \
{ "s32", NULL, MAVLINK_TYPE_INT32_T, 0, 100, offsetof(mavlink_test_types_t, s32) }, \
{ "f", NULL, MAVLINK_TYPE_FLOAT, 0, 104, offsetof(mavlink_test_types_t, f) }, \
{ "u32_array", NULL, MAVLINK_TYPE_UINT32_T, 3, 108, offsetof(mavlink_test_types_t, u32_array) }, \
{ "s32_array", NULL, MAVLINK_TYPE_INT32_T, 3, 120, offsetof(mavlink_test_types_t, s32_array) }, \
{ "f_array", NULL, MAVLINK_TYPE_FLOAT, 3, 132, offsetof(mavlink_test_types_t, f_array) }, \
{ "u16", NULL, MAVLINK_TYPE_UINT16_T, 0, 144, offsetof(mavlink_test_types_t, u16) }, \
{ "s16", NULL, MAVLINK_TYPE_INT16_T, 0, 146, offsetof(mavlink_test_types_t, s16) }, \
{ "u16_array", NULL, MAVLINK_TYPE_UINT16_T, 3, 148, offsetof(mavlink_test_types_t, u16_array) }, \
{ "s16_array", NULL, MAVLINK_TYPE_INT16_T, 3, 154, offsetof(mavlink_test_types_t, s16_array) }, \
{ "c", NULL, MAVLINK_TYPE_CHAR, 0, 160, offsetof(mavlink_test_types_t, c) }, \
{ "s", NULL, MAVLINK_TYPE_CHAR, 10, 161, offsetof(mavlink_test_types_t, s) }, \
{ "u8", NULL, MAVLINK_TYPE_UINT8_T, 0, 171, offsetof(mavlink_test_types_t, u8) }, \
{ "s8", NULL, MAVLINK_TYPE_INT8_T, 0, 172, offsetof(mavlink_test_types_t, s8) }, \
{ "u8_array", NULL, MAVLINK_TYPE_UINT8_T, 3, 173, offsetof(mavlink_test_types_t, u8_array) }, \
{ "s8_array", NULL, MAVLINK_TYPE_INT8_T, 3, 176, offsetof(mavlink_test_types_t, s8_array) }, \
} \
}
/**
* @brief Pack a test_types message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_test_types_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_uint64_t(buf, 0, u64);
_mav_put_int64_t(buf, 8, s64);
_mav_put_double(buf, 16, d);
_mav_put_uint32_t(buf, 96, u32);
_mav_put_int32_t(buf, 100, s32);
_mav_put_float(buf, 104, f);
_mav_put_uint16_t(buf, 144, u16);
_mav_put_int16_t(buf, 146, s16);
_mav_put_char(buf, 160, c);
_mav_put_uint8_t(buf, 171, u8);
_mav_put_int8_t(buf, 172, s8);
_mav_put_uint64_t_array(buf, 24, u64_array, 3);
_mav_put_int64_t_array(buf, 48, s64_array, 3);
_mav_put_double_array(buf, 72, d_array, 3);
_mav_put_uint32_t_array(buf, 108, u32_array, 3);
_mav_put_int32_t_array(buf, 120, s32_array, 3);
_mav_put_float_array(buf, 132, f_array, 3);
_mav_put_uint16_t_array(buf, 148, u16_array, 3);
_mav_put_int16_t_array(buf, 154, s16_array, 3);
_mav_put_char_array(buf, 161, s, 10);
_mav_put_uint8_t_array(buf, 173, u8_array, 3);
_mav_put_int8_t_array(buf, 176, s8_array, 3);
memcpy(_MAV_PAYLOAD(msg), buf, 179);
#else
mavlink_test_types_t packet;
packet.u64 = u64;
packet.s64 = s64;
packet.d = d;
packet.u32 = u32;
packet.s32 = s32;
packet.f = f;
packet.u16 = u16;
packet.s16 = s16;
packet.c = c;
packet.u8 = u8;
packet.s8 = s8;
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
memcpy(_MAV_PAYLOAD(msg), &packet, 179);
#endif
msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
return mavlink_finalize_message(msg, system_id, component_id, 179, 103);
}
/**
* @brief Pack a test_types message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_test_types_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
char c,const char *s,uint8_t u8,uint16_t u16,uint32_t u32,uint64_t u64,int8_t s8,int16_t s16,int32_t s32,int64_t s64,float f,double d,const uint8_t *u8_array,const uint16_t *u16_array,const uint32_t *u32_array,const uint64_t *u64_array,const int8_t *s8_array,const int16_t *s16_array,const int32_t *s32_array,const int64_t *s64_array,const float *f_array,const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_uint64_t(buf, 0, u64);
_mav_put_int64_t(buf, 8, s64);
_mav_put_double(buf, 16, d);
_mav_put_uint32_t(buf, 96, u32);
_mav_put_int32_t(buf, 100, s32);
_mav_put_float(buf, 104, f);
_mav_put_uint16_t(buf, 144, u16);
_mav_put_int16_t(buf, 146, s16);
_mav_put_char(buf, 160, c);
_mav_put_uint8_t(buf, 171, u8);
_mav_put_int8_t(buf, 172, s8);
_mav_put_uint64_t_array(buf, 24, u64_array, 3);
_mav_put_int64_t_array(buf, 48, s64_array, 3);
_mav_put_double_array(buf, 72, d_array, 3);
_mav_put_uint32_t_array(buf, 108, u32_array, 3);
_mav_put_int32_t_array(buf, 120, s32_array, 3);
_mav_put_float_array(buf, 132, f_array, 3);
_mav_put_uint16_t_array(buf, 148, u16_array, 3);
_mav_put_int16_t_array(buf, 154, s16_array, 3);
_mav_put_char_array(buf, 161, s, 10);
_mav_put_uint8_t_array(buf, 173, u8_array, 3);
_mav_put_int8_t_array(buf, 176, s8_array, 3);
memcpy(_MAV_PAYLOAD(msg), buf, 179);
#else
mavlink_test_types_t packet;
packet.u64 = u64;
packet.s64 = s64;
packet.d = d;
packet.u32 = u32;
packet.s32 = s32;
packet.f = f;
packet.u16 = u16;
packet.s16 = s16;
packet.c = c;
packet.u8 = u8;
packet.s8 = s8;
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
memcpy(_MAV_PAYLOAD(msg), &packet, 179);
#endif
msg->msgid = MAVLINK_MSG_ID_TEST_TYPES;
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 179, 103);
}
/**
* @brief Encode a test_types struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param test_types C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_test_types_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_test_types_t* test_types)
{
return mavlink_msg_test_types_pack(system_id, component_id, msg, test_types->c, test_types->s, test_types->u8, test_types->u16, test_types->u32, test_types->u64, test_types->s8, test_types->s16, test_types->s32, test_types->s64, test_types->f, test_types->d, test_types->u8_array, test_types->u16_array, test_types->u32_array, test_types->u64_array, test_types->s8_array, test_types->s16_array, test_types->s32_array, test_types->s64_array, test_types->f_array, test_types->d_array);
}
/**
* @brief Send a test_types message
* @param chan MAVLink channel to send the message
*
* @param c char
* @param s string
* @param u8 uint8_t
* @param u16 uint16_t
* @param u32 uint32_t
* @param u64 uint64_t
* @param s8 int8_t
* @param s16 int16_t
* @param s32 int32_t
* @param s64 int64_t
* @param f float
* @param d double
* @param u8_array uint8_t_array
* @param u16_array uint16_t_array
* @param u32_array uint32_t_array
* @param u64_array uint64_t_array
* @param s8_array int8_t_array
* @param s16_array int16_t_array
* @param s32_array int32_t_array
* @param s64_array int64_t_array
* @param f_array float_array
* @param d_array double_array
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_test_types_send(mavlink_channel_t chan, char c, const char *s, uint8_t u8, uint16_t u16, uint32_t u32, uint64_t u64, int8_t s8, int16_t s16, int32_t s32, int64_t s64, float f, double d, const uint8_t *u8_array, const uint16_t *u16_array, const uint32_t *u32_array, const uint64_t *u64_array, const int8_t *s8_array, const int16_t *s16_array, const int32_t *s32_array, const int64_t *s64_array, const float *f_array, const double *d_array)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[179];
_mav_put_uint64_t(buf, 0, u64);
_mav_put_int64_t(buf, 8, s64);
_mav_put_double(buf, 16, d);
_mav_put_uint32_t(buf, 96, u32);
_mav_put_int32_t(buf, 100, s32);
_mav_put_float(buf, 104, f);
_mav_put_uint16_t(buf, 144, u16);
_mav_put_int16_t(buf, 146, s16);
_mav_put_char(buf, 160, c);
_mav_put_uint8_t(buf, 171, u8);
_mav_put_int8_t(buf, 172, s8);
_mav_put_uint64_t_array(buf, 24, u64_array, 3);
_mav_put_int64_t_array(buf, 48, s64_array, 3);
_mav_put_double_array(buf, 72, d_array, 3);
_mav_put_uint32_t_array(buf, 108, u32_array, 3);
_mav_put_int32_t_array(buf, 120, s32_array, 3);
_mav_put_float_array(buf, 132, f_array, 3);
_mav_put_uint16_t_array(buf, 148, u16_array, 3);
_mav_put_int16_t_array(buf, 154, s16_array, 3);
_mav_put_char_array(buf, 161, s, 10);
_mav_put_uint8_t_array(buf, 173, u8_array, 3);
_mav_put_int8_t_array(buf, 176, s8_array, 3);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, buf, 179, 103);
#else
mavlink_test_types_t packet;
packet.u64 = u64;
packet.s64 = s64;
packet.d = d;
packet.u32 = u32;
packet.s32 = s32;
packet.f = f;
packet.u16 = u16;
packet.s16 = s16;
packet.c = c;
packet.u8 = u8;
packet.s8 = s8;
mav_array_memcpy(packet.u64_array, u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet.s64_array, s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet.d_array, d_array, sizeof(double)*3);
mav_array_memcpy(packet.u32_array, u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet.s32_array, s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet.f_array, f_array, sizeof(float)*3);
mav_array_memcpy(packet.u16_array, u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet.s16_array, s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet.s, s, sizeof(char)*10);
mav_array_memcpy(packet.u8_array, u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet.s8_array, s8_array, sizeof(int8_t)*3);
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TEST_TYPES, (const char *)&packet, 179, 103);
#endif
}
#endif
// MESSAGE TEST_TYPES UNPACKING
/**
* @brief Get field c from test_types message
*
* @return char
*/
static inline char mavlink_msg_test_types_get_c(const mavlink_message_t* msg)
{
return _MAV_RETURN_char(msg, 160);
}
/**
* @brief Get field s from test_types message
*
* @return string
*/
static inline uint16_t mavlink_msg_test_types_get_s(const mavlink_message_t* msg, char *s)
{
return _MAV_RETURN_char_array(msg, s, 10, 161);
}
/**
* @brief Get field u8 from test_types message
*
* @return uint8_t
*/
static inline uint8_t mavlink_msg_test_types_get_u8(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint8_t(msg, 171);
}
/**
* @brief Get field u16 from test_types message
*
* @return uint16_t
*/
static inline uint16_t mavlink_msg_test_types_get_u16(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint16_t(msg, 144);
}
/**
* @brief Get field u32 from test_types message
*
* @return uint32_t
*/
static inline uint32_t mavlink_msg_test_types_get_u32(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint32_t(msg, 96);
}
/**
* @brief Get field u64 from test_types message
*
* @return uint64_t
*/
static inline uint64_t mavlink_msg_test_types_get_u64(const mavlink_message_t* msg)
{
return _MAV_RETURN_uint64_t(msg, 0);
}
/**
* @brief Get field s8 from test_types message
*
* @return int8_t
*/
static inline int8_t mavlink_msg_test_types_get_s8(const mavlink_message_t* msg)
{
return _MAV_RETURN_int8_t(msg, 172);
}
/**
* @brief Get field s16 from test_types message
*
* @return int16_t
*/
static inline int16_t mavlink_msg_test_types_get_s16(const mavlink_message_t* msg)
{
return _MAV_RETURN_int16_t(msg, 146);
}
/**
* @brief Get field s32 from test_types message
*
* @return int32_t
*/
static inline int32_t mavlink_msg_test_types_get_s32(const mavlink_message_t* msg)
{
return _MAV_RETURN_int32_t(msg, 100);
}
/**
* @brief Get field s64 from test_types message
*
* @return int64_t
*/
static inline int64_t mavlink_msg_test_types_get_s64(const mavlink_message_t* msg)
{
return _MAV_RETURN_int64_t(msg, 8);
}
/**
* @brief Get field f from test_types message
*
* @return float
*/
static inline float mavlink_msg_test_types_get_f(const mavlink_message_t* msg)
{
return _MAV_RETURN_float(msg, 104);
}
/**
* @brief Get field d from test_types message
*
* @return double
*/
static inline double mavlink_msg_test_types_get_d(const mavlink_message_t* msg)
{
return _MAV_RETURN_double(msg, 16);
}
/**
* @brief Get field u8_array from test_types message
*
* @return uint8_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u8_array(const mavlink_message_t* msg, uint8_t *u8_array)
{
return _MAV_RETURN_uint8_t_array(msg, u8_array, 3, 173);
}
/**
* @brief Get field u16_array from test_types message
*
* @return uint16_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u16_array(const mavlink_message_t* msg, uint16_t *u16_array)
{
return _MAV_RETURN_uint16_t_array(msg, u16_array, 3, 148);
}
/**
* @brief Get field u32_array from test_types message
*
* @return uint32_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u32_array(const mavlink_message_t* msg, uint32_t *u32_array)
{
return _MAV_RETURN_uint32_t_array(msg, u32_array, 3, 108);
}
/**
* @brief Get field u64_array from test_types message
*
* @return uint64_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_u64_array(const mavlink_message_t* msg, uint64_t *u64_array)
{
return _MAV_RETURN_uint64_t_array(msg, u64_array, 3, 24);
}
/**
* @brief Get field s8_array from test_types message
*
* @return int8_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s8_array(const mavlink_message_t* msg, int8_t *s8_array)
{
return _MAV_RETURN_int8_t_array(msg, s8_array, 3, 176);
}
/**
* @brief Get field s16_array from test_types message
*
* @return int16_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s16_array(const mavlink_message_t* msg, int16_t *s16_array)
{
return _MAV_RETURN_int16_t_array(msg, s16_array, 3, 154);
}
/**
* @brief Get field s32_array from test_types message
*
* @return int32_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s32_array(const mavlink_message_t* msg, int32_t *s32_array)
{
return _MAV_RETURN_int32_t_array(msg, s32_array, 3, 120);
}
/**
* @brief Get field s64_array from test_types message
*
* @return int64_t_array
*/
static inline uint16_t mavlink_msg_test_types_get_s64_array(const mavlink_message_t* msg, int64_t *s64_array)
{
return _MAV_RETURN_int64_t_array(msg, s64_array, 3, 48);
}
/**
* @brief Get field f_array from test_types message
*
* @return float_array
*/
static inline uint16_t mavlink_msg_test_types_get_f_array(const mavlink_message_t* msg, float *f_array)
{
return _MAV_RETURN_float_array(msg, f_array, 3, 132);
}
/**
* @brief Get field d_array from test_types message
*
* @return double_array
*/
static inline uint16_t mavlink_msg_test_types_get_d_array(const mavlink_message_t* msg, double *d_array)
{
return _MAV_RETURN_double_array(msg, d_array, 3, 72);
}
/**
* @brief Decode a test_types message into a struct
*
* @param msg The message to decode
* @param test_types C-struct to decode the message contents into
*/
static inline void mavlink_msg_test_types_decode(const mavlink_message_t* msg, mavlink_test_types_t* test_types)
{
#if MAVLINK_NEED_BYTE_SWAP
test_types->u64 = mavlink_msg_test_types_get_u64(msg);
test_types->s64 = mavlink_msg_test_types_get_s64(msg);
test_types->d = mavlink_msg_test_types_get_d(msg);
mavlink_msg_test_types_get_u64_array(msg, test_types->u64_array);
mavlink_msg_test_types_get_s64_array(msg, test_types->s64_array);
mavlink_msg_test_types_get_d_array(msg, test_types->d_array);
test_types->u32 = mavlink_msg_test_types_get_u32(msg);
test_types->s32 = mavlink_msg_test_types_get_s32(msg);
test_types->f = mavlink_msg_test_types_get_f(msg);
mavlink_msg_test_types_get_u32_array(msg, test_types->u32_array);
mavlink_msg_test_types_get_s32_array(msg, test_types->s32_array);
mavlink_msg_test_types_get_f_array(msg, test_types->f_array);
test_types->u16 = mavlink_msg_test_types_get_u16(msg);
test_types->s16 = mavlink_msg_test_types_get_s16(msg);
mavlink_msg_test_types_get_u16_array(msg, test_types->u16_array);
mavlink_msg_test_types_get_s16_array(msg, test_types->s16_array);
test_types->c = mavlink_msg_test_types_get_c(msg);
mavlink_msg_test_types_get_s(msg, test_types->s);
test_types->u8 = mavlink_msg_test_types_get_u8(msg);
test_types->s8 = mavlink_msg_test_types_get_s8(msg);
mavlink_msg_test_types_get_u8_array(msg, test_types->u8_array);
mavlink_msg_test_types_get_s8_array(msg, test_types->s8_array);
#else
memcpy(test_types, _MAV_PAYLOAD(msg), 179);
#endif
}
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/test.h
-53
View File
@@ -1,53 +0,0 @@
/** @file
* @brief MAVLink comm protocol generated from test.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef TEST_H
#define TEST_H
#ifdef __cplusplus
extern "C" {
#endif
// MESSAGE LENGTHS AND CRCS
#ifndef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS {179, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {103, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
#endif
#ifndef MAVLINK_MESSAGE_INFO
#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_TEST_TYPES, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}}
#endif
#include "../protocol.h"
#define MAVLINK_ENABLED_TEST
// MAVLINK VERSION
#ifndef MAVLINK_VERSION
#define MAVLINK_VERSION 3
#endif
#if (MAVLINK_VERSION == 0)
#undef MAVLINK_VERSION
#define MAVLINK_VERSION 3
#endif
// ENUM DEFINITIONS
// MESSAGE DEFINITIONS
#include "./mavlink_msg_test_types.h"
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // TEST_H
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/testsuite.h
-120
View File
@@ -1,120 +0,0 @@
/** @file
* @brief MAVLink comm protocol testsuite generated from test.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef TEST_TESTSUITE_H
#define TEST_TESTSUITE_H
#ifdef __cplusplus
extern "C" {
#endif
#ifndef MAVLINK_TEST_ALL
#define MAVLINK_TEST_ALL
static void mavlink_test_test(uint8_t, uint8_t, mavlink_message_t *last_msg);
static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_test(system_id, component_id, last_msg);
}
#endif
static void mavlink_test_test_types(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_test_types_t packet_in = {
93372036854775807ULL,
93372036854776311LL,
235.0,
{ 93372036854777319, 93372036854777320, 93372036854777321 },
{ 93372036854778831, 93372036854778832, 93372036854778833 },
{ 627.0, 628.0, 629.0 },
963502456,
963502664,
745.0,
{ 963503080, 963503081, 963503082 },
{ 963503704, 963503705, 963503706 },
{ 941.0, 942.0, 943.0 },
24723,
24827,
{ 24931, 24932, 24933 },
{ 25243, 25244, 25245 },
'E',
"FGHIJKLMN",
198,
9,
{ 76, 77, 78 },
{ 21, 22, 23 },
};
mavlink_test_types_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.u64 = packet_in.u64;
packet1.s64 = packet_in.s64;
packet1.d = packet_in.d;
packet1.u32 = packet_in.u32;
packet1.s32 = packet_in.s32;
packet1.f = packet_in.f;
packet1.u16 = packet_in.u16;
packet1.s16 = packet_in.s16;
packet1.c = packet_in.c;
packet1.u8 = packet_in.u8;
packet1.s8 = packet_in.s8;
mav_array_memcpy(packet1.u64_array, packet_in.u64_array, sizeof(uint64_t)*3);
mav_array_memcpy(packet1.s64_array, packet_in.s64_array, sizeof(int64_t)*3);
mav_array_memcpy(packet1.d_array, packet_in.d_array, sizeof(double)*3);
mav_array_memcpy(packet1.u32_array, packet_in.u32_array, sizeof(uint32_t)*3);
mav_array_memcpy(packet1.s32_array, packet_in.s32_array, sizeof(int32_t)*3);
mav_array_memcpy(packet1.f_array, packet_in.f_array, sizeof(float)*3);
mav_array_memcpy(packet1.u16_array, packet_in.u16_array, sizeof(uint16_t)*3);
mav_array_memcpy(packet1.s16_array, packet_in.s16_array, sizeof(int16_t)*3);
mav_array_memcpy(packet1.s, packet_in.s, sizeof(char)*10);
mav_array_memcpy(packet1.u8_array, packet_in.u8_array, sizeof(uint8_t)*3);
mav_array_memcpy(packet1.s8_array, packet_in.s8_array, sizeof(int8_t)*3);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_pack(system_id, component_id, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_test_types_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_test_types_send(MAVLINK_COMM_1 , packet1.c , packet1.s , packet1.u8 , packet1.u16 , packet1.u32 , packet1.u64 , packet1.s8 , packet1.s16 , packet1.s32 , packet1.s64 , packet1.f , packet1.d , packet1.u8_array , packet1.u16_array , packet1.u32_array , packet1.u64_array , packet1.s8_array , packet1.s16_array , packet1.s32_array , packet1.s64_array , packet1.f_array , packet1.d_array );
mavlink_msg_test_types_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_test(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_test_test_types(system_id, component_id, last_msg);
}
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // TEST_TESTSUITE_H
mavlink/share/pyshared/pymavlink/generator/C/include_v1.0/test/version.h
-12
View File
@@ -1,12 +0,0 @@
/** @file
* @brief MAVLink comm protocol built from test.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Thu Mar 1 15:11:58 2012"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 179
#endif // MAVLINK_VERSION_H
mavlink/share/pyshared/pymavlink/generator/C/src_v1.0/pixhawk/pixhawk.pb.cc
-5431
View File
File diff suppressed because it is too large Load Diff
mavlink/share/pyshared/pymavlink/generator/C/test/posix/.gitignore
-3
View File
@@ -1,3 +0,0 @@
/testmav0.9
/testmav1.0
/testmav1.0_nonstrict
mavlink/share/pyshared/pymavlink/generator/C/test/posix/testmav.c
-159
View File
@@ -1,159 +0,0 @@
/*
simple MAVLink testsuite for C
*/
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <assert.h>
#include <stddef.h>
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_COMM_NUM_BUFFERS 2
// this trick allows us to make mavlink_message_t as small as possible
// for this dialect, which saves some memory
#include <version.h>
#define MAVLINK_MAX_PAYLOAD_LEN MAVLINK_MAX_DIALECT_PAYLOAD_SIZE
#include <mavlink_types.h>
static mavlink_system_t mavlink_system = {42,11,};
#define MAVLINK_ASSERT(x) assert(x)
static void comm_send_ch(mavlink_channel_t chan, uint8_t c);
static mavlink_message_t last_msg;
#include <mavlink.h>
#include <testsuite.h>
static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
static unsigned error_count;
static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;
static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
{
#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
switch (f->type) {
case MAVLINK_TYPE_CHAR:
printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_UINT8_T:
printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_INT8_T:
printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_UINT16_T:
printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
break;
case MAVLINK_TYPE_INT16_T:
printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
break;
case MAVLINK_TYPE_UINT32_T:
printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_INT32_T:
printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_UINT64_T:
printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
break;
case MAVLINK_TYPE_INT64_T:
printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
break;
case MAVLINK_TYPE_FLOAT:
printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_DOUBLE:
printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
break;
}
}
static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)
{
printf("%s: ", f->name);
if (f->array_length == 0) {
print_one_field(msg, f, 0);
printf(" ");
} else {
unsigned i;
/* print an array */
if (f->type == MAVLINK_TYPE_CHAR) {
printf("'%.*s'", f->array_length,
f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
} else {
printf("[ ");
for (i=0; i<f->array_length; i++) {
print_one_field(msg, f, i);
if (i < f->array_length) {
printf(", ");
}
}
printf("]");
}
}
printf(" ");
}
static void print_message(mavlink_message_t *msg)
{
const mavlink_message_info_t *m = &message_info[msg->msgid];
const mavlink_field_info_t *f = m->fields;
unsigned i;
printf("%s { ", m->name);
for (i=0; i<m->num_fields; i++) {
print_field(msg, &f[i]);
}
printf("}\n");
}
static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
{
mavlink_status_t status;
if (mavlink_parse_char(chan, c, &last_msg, &status)) {
print_message(&last_msg);
chan_counts[chan]++;
/* channel 0 gets 3 messages per message, because of
the channel defaults for _pack() and _encode() */
if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n",
chan_counts[chan], status.current_rx_seq);
error_count++;
} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n",
(unsigned)chan, chan_counts[chan], status.current_rx_seq);
error_count++;
}
if (message_lengths[last_msg.msgid] != last_msg.len) {
printf("Incorrect message length %u for message %u - expected %u\n",
(unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);
error_count++;
}
}
if (status.packet_rx_drop_count != 0) {
printf("Parse error at packet %u\n", chan_counts[chan]);
error_count++;
}
}
int main(void)
{
mavlink_channel_t chan;
mavlink_test_all(11, 10, &last_msg);
for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
printf("Received %u messages on channel %u OK\n",
chan_counts[chan], (unsigned)chan);
}
if (error_count != 0) {
printf("Error count %u\n", error_count);
exit(1);
}
printf("No errors detected\n");
return 0;
}
mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.cpp
-8
View File
@@ -1,8 +0,0 @@
// stdafx.cpp : source file that includes just the standard includes
// testmav.pch will be the pre-compiled header
// stdafx.obj will contain the pre-compiled type information
#include "stdafx.h"
// TODO: reference any additional headers you need in STDAFX.H
// and not in this file
mavlink/share/pyshared/pymavlink/generator/C/test/windows/stdafx.h
-15
View File
@@ -1,15 +0,0 @@
// stdafx.h : include file for standard system include files,
// or project specific include files that are used frequently, but
// are changed infrequently
//
#pragma once
#include "targetver.h"
#include <stdio.h>
#include <tchar.h>
// TODO: reference additional headers your program requires here
mavlink/share/pyshared/pymavlink/generator/C/test/windows/targetver.h
-8
View File
@@ -1,8 +0,0 @@
#pragma once
// Including SDKDDKVer.h defines the highest available Windows platform.
// If you wish to build your application for a previous Windows platform, include WinSDKVer.h and
// set the _WIN32_WINNT macro to the platform you wish to support before including SDKDDKVer.h.
#include <SDKDDKVer.h>
mavlink/share/pyshared/pymavlink/generator/C/test/windows/testmav.cpp
-154
View File
@@ -1,154 +0,0 @@
// testmav.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#include "stdio.h"
#include "stdint.h"
#include "stddef.h"
#include "assert.h"
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_COMM_NUM_BUFFERS 2
#include <mavlink_types.h>
static mavlink_system_t mavlink_system = {42,11,};
#define MAVLINK_ASSERT(x) assert(x)
static void comm_send_ch(mavlink_channel_t chan, uint8_t c);
static mavlink_message_t last_msg;
#include <common/mavlink.h>
#include <common/testsuite.h>
static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
static unsigned error_count;
static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;
static void print_one_field(mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
{
#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
switch (f->type) {
case MAVLINK_TYPE_CHAR:
printf(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_UINT8_T:
printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_INT8_T:
printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
break;
case MAVLINK_TYPE_UINT16_T:
printf(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
break;
case MAVLINK_TYPE_INT16_T:
printf(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
break;
case MAVLINK_TYPE_UINT32_T:
printf(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_INT32_T:
printf(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_UINT64_T:
printf(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
break;
case MAVLINK_TYPE_INT64_T:
printf(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
break;
case MAVLINK_TYPE_FLOAT:
printf(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
break;
case MAVLINK_TYPE_DOUBLE:
printf(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
break;
}
}
static void print_field(mavlink_message_t *msg, const mavlink_field_info_t *f)
{
printf("%s: ", f->name);
if (f->array_length == 0) {
print_one_field(msg, f, 0);
printf(" ");
} else {
unsigned i;
/* print an array */
if (f->type == MAVLINK_TYPE_CHAR) {
printf("'%.*s'", f->array_length,
f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
} else {
printf("[ ");
for (i=0; i<f->array_length; i++) {
print_one_field(msg, f, i);
if (i < f->array_length) {
printf(", ");
}
}
printf("]");
}
}
printf(" ");
}
static void print_message(mavlink_message_t *msg)
{
const mavlink_message_info_t *m = &message_info[msg->msgid];
const mavlink_field_info_t *f = m->fields;
unsigned i;
printf("%s { ", m->name);
for (i=0; i<m->num_fields; i++) {
print_field(msg, &f[i]);
}
printf("}\n");
}
static void comm_send_ch(mavlink_channel_t chan, uint8_t c)
{
mavlink_status_t status;
if (mavlink_parse_char(chan, c, &last_msg, &status)) {
print_message(&last_msg);
chan_counts[chan]++;
/* channel 0 gets 3 messages per message, because of
the channel defaults for _pack() and _encode() */
if (chan == MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)(chan_counts[chan]*3)) {
printf("Channel 0 sequence mismatch error at packet %u (rx_seq=%u)\n",
chan_counts[chan], status.current_rx_seq);
error_count++;
} else if (chan > MAVLINK_COMM_0 && status.current_rx_seq != (uint8_t)chan_counts[chan]) {
printf("Channel %u sequence mismatch error at packet %u (rx_seq=%u)\n",
(unsigned)chan, chan_counts[chan], status.current_rx_seq);
error_count++;
}
if (message_lengths[last_msg.msgid] != last_msg.len) {
printf("Incorrect message length %u for message %u - expected %u\n",
(unsigned)last_msg.len, (unsigned)last_msg.msgid, message_lengths[last_msg.msgid]);
error_count++;
}
}
if (status.packet_rx_drop_count != 0) {
printf("Parse error at packet %u\n", chan_counts[chan]);
error_count++;
}
}
int _tmain(int argc, _TCHAR* argv[])
{
int chan;
mavlink_test_all(11, 10, &last_msg);
for (chan=MAVLINK_COMM_0; chan<=MAVLINK_COMM_1; chan++) {
printf("Received %u messages on channel %u OK\n",
chan_counts[chan], (unsigned)chan);
}
if (error_count != 0) {
printf("Error count %u\n", error_count);
return(1);
}
printf("No errors detected\n");
return 0;
}
mavlink/share/pyshared/pymavlink/generator/gen_MatrixPilot.py
-93
View File
@@ -1,93 +0,0 @@
#!/usr/bin/env python
'''
Use mavgen.py matrixpilot.xml definitions to generate
C and Python MAVLink routines for sending and parsing the protocol
This python script is soley for MatrixPilot MAVLink impoementation
Copyright Pete Hollands 2011
Released under GNU GPL version 3 or later
'''
import os, sys, glob, re
from shutil import copy
from mavgen import mavgen
# allow import from the parent directory, where mavutil.py is
# Under Windows, this script must be run from a DOS command window
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
class options:
""" a class to simulate the options of mavgen OptionsParser"""
def __init__(self, lang, output, wire_protocol):
self.language = lang
self.wire_protocol = wire_protocol
self.output = output
def remove_include_files(target_directory):
search_pattern = target_directory+'/*.h'
print "search pattern is", search_pattern
files_to_remove = glob.glob(search_pattern)
for afile in files_to_remove :
try:
print "removing", afile
os.remove(afile)
except:
print "error while trying to remove", afile
def copy_include_files(source_directory,target_directory):
search_pattern = source_directory+'/*.h'
files_to_copy = glob.glob(search_pattern)
for afile in files_to_copy:
basename = os.path.basename(afile)
print "Copying ...", basename
copy(afile, target_directory)
protocol = "1.0"
xml_directory = './message_definitions/v'+protocol
print "xml_directory is", xml_directory
xml_file_names = []
xml_file_names.append(xml_directory+"/"+"matrixpilot.xml")
for xml_file in xml_file_names:
print "xml file is ", xml_file
opts = options(lang = "C", output = "C/include_v"+protocol, \
wire_protocol=protocol)
args = []
args.append(xml_file)
mavgen(opts, args)
xml_file_base = os.path.basename(xml_file)
xml_file_base = re.sub("\.xml","", xml_file_base)
print "xml_file_base is", xml_file_base
opts = options(lang = "python", \
output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \
wire_protocol=protocol)
mavgen(opts,args)
mavlink_directory_list = ["common","matrixpilot"]
for mavlink_directory in mavlink_directory_list :
# Look specifically for MatrixPilot directory structure
target_directory = "../../../../MAVLink/include/"+mavlink_directory
source_directory = "C/include_v"+protocol+"/"+mavlink_directory
if os.access(source_directory, os.R_OK):
if os.access(target_directory, os.W_OK):
print "Preparing to copy over files..."
print "About to remove all files in",target_directory
print "OK to continue ?[Yes / No]: ",
line = sys.stdin.readline()
if line == "Yes\n" or line == "yes\n" \
or line == "Y\n" or line == "y\n":
print "passed"
remove_include_files(target_directory)
copy_include_files(source_directory,target_directory)
print "Finished copying over include files"
else :
print "Your answer is No. Exiting Program"
sys.exit()
else :
print "Cannot find " + target_directory + "in MatrixPilot"
sys.exit()
else:
print "Could not find files to copy at", source_directory
print "Exiting Program."
sys.exit()
mavlink/share/pyshared/pymavlink/generator/gen_all.py
-44
View File
@@ -1,44 +0,0 @@
#!/usr/bin/env python
'''
Use mavgen.py on all available MAVLink XML definitions to generate
C and Python MAVLink routines for sending and parsing the protocol
Copyright Pete Hollands 2011
Released under GNU GPL version 3 or later
'''
import os, sys, glob, re
from mavgen import mavgen
# allow import from the parent directory, where mavutil.py is
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
class options:
""" a class to simulate the options of mavgen OptionsParser"""
def __init__(self, lang, output, wire_protocol):
self.language = lang
self.wire_protocol = wire_protocol
self.output = output
protocols = [ '0.9', '1.0' ]
for protocol in protocols :
xml_directory = './message_definitions/v'+protocol
print "xml_directory is", xml_directory
xml_file_names = glob.glob(xml_directory+'/*.xml')
for xml_file in xml_file_names:
print "xml file is ", xml_file
opts = options(lang = "C", output = "C/include_v"+protocol, \
wire_protocol=protocol)
args = []
args.append(xml_file)
mavgen(opts, args)
xml_file_base = os.path.basename(xml_file)
xml_file_base = re.sub("\.xml","", xml_file_base)
print "xml_file_base is", xml_file_base
opts = options(lang = "python", \
output="python/mavlink_"+xml_file_base+"_v"+protocol+".py", \
wire_protocol=protocol)
mavgen(opts,args)
mavlink/share/pyshared/pymavlink/generator/gen_all.sh
-12
View File
@@ -1,12 +0,0 @@
#!/bin/sh
for protocol in 0.9 1.0; do
for xml in message_definitions/v$protocol/*.xml; do
base=$(basename $xml .xml)
./mavgen.py --lang=C --wire-protocol=$protocol --output=C/include_v$protocol $xml || exit 1
./mavgen.py --lang=python --wire-protocol=$protocol --output=python/mavlink_${base}_v$protocol.py $xml || exit 1
done
done
cp -f python/mavlink_ardupilotmega_v0.9.py ../mavlink.py
cp -f python/mavlink_ardupilotmega_v1.0.py ../mavlinkv10.py
mavlink/share/pyshared/pymavlink/generator/mavgen.py
-82
View File
@@ -1,82 +0,0 @@
#!/usr/bin/env python
'''
parse a MAVLink protocol XML file and generate a python implementation
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
def mavgen(opts, args) :
"""Generate mavlink message formatters and parsers (C and Python ) using options
and args where args are a list of xml files. This function allows python
scripts under Windows to control mavgen using the same interface as
shell scripts under Unix"""
import sys, textwrap, os
import mavparse
import mavgen_python
import mavgen_c
xml = []
for fname in args:
print("Parsing %s" % fname)
xml.append(mavparse.MAVXML(fname, opts.wire_protocol))
# expand includes
for x in xml[:]:
for i in x.include:
fname = os.path.join(os.path.dirname(x.filename), i)
print("Parsing %s" % fname)
xml.append(mavparse.MAVXML(fname, opts.wire_protocol))
# include message lengths and CRCs too
for idx in range(0, 256):
if x.message_lengths[idx] == 0:
x.message_lengths[idx] = xml[-1].message_lengths[idx]
x.message_crcs[idx] = xml[-1].message_crcs[idx]
x.message_names[idx] = xml[-1].message_names[idx]
# work out max payload size across all includes
largest_payload = 0
for x in xml:
if x.largest_payload > largest_payload:
largest_payload = x.largest_payload
for x in xml:
x.largest_payload = largest_payload
if mavparse.check_duplicates(xml):
sys.exit(1)
print("Found %u MAVLink message types in %u XML files" % (
mavparse.total_msgs(xml), len(xml)))
if opts.language == 'python':
mavgen_python.generate(opts.output, xml)
elif opts.language == 'C':
mavgen_c.generate(opts.output, xml)
else:
print("Unsupported language %s" % opts.language)
if __name__=="__main__":
import sys, textwrap, os
from optparse import OptionParser
# allow import from the parent directory, where mavutil.py is
sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..'))
import mavparse
import mavgen_python
import mavgen_c
parser = OptionParser("%prog [options] <XML files>")
parser.add_option("-o", "--output", dest="output", default="mavlink", help="output directory.")
parser.add_option("--lang", dest="language", default="python", help="language of generated code: 'Python' or 'C' [default: %default]")
parser.add_option("--wire-protocol", dest="wire_protocol", default=mavparse.PROTOCOL_0_9, help="MAVLink protocol version: '0.9' or '1.0'. [default: %default]")
(opts, args) = parser.parse_args()
if len(args) < 1:
parser.error("You must supply at least one MAVLink XML protocol definition")
mavgen(opts, args)
mavlink/share/pyshared/pymavlink/generator/mavgen_c.py
-581
View File
@@ -1,581 +0,0 @@
#!/usr/bin/env python
'''
parse a MAVLink protocol XML file and generate a C implementation
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
import sys, textwrap, os, time
import mavparse, mavtemplate
t = mavtemplate.MAVTemplate()
def generate_version_h(directory, xml):
'''generate version.h'''
f = open(os.path.join(directory, "version.h"), mode='w')
t.write(f,'''
/** @file
* @brief MAVLink comm protocol built from ${basename}.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "${parse_time}"
#define MAVLINK_WIRE_PROTOCOL_VERSION "${wire_protocol_version}"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE ${largest_payload}
#endif // MAVLINK_VERSION_H
''', xml)
f.close()
def generate_mavlink_h(directory, xml):
'''generate mavlink.h'''
f = open(os.path.join(directory, "mavlink.h"), mode='w')
t.write(f,'''
/** @file
* @brief MAVLink comm protocol built from ${basename}.xml
* @see http://pixhawk.ethz.ch/software/mavlink
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#ifndef MAVLINK_STX
#define MAVLINK_STX ${protocol_marker}
#endif
#ifndef MAVLINK_ENDIAN
#define MAVLINK_ENDIAN ${mavlink_endian}
#endif
#ifndef MAVLINK_ALIGNED_FIELDS
#define MAVLINK_ALIGNED_FIELDS ${aligned_fields_define}
#endif
#ifndef MAVLINK_CRC_EXTRA
#define MAVLINK_CRC_EXTRA ${crc_extra_define}
#endif
#include "version.h"
#include "${basename}.h"
#endif // MAVLINK_H
''', xml)
f.close()
def generate_main_h(directory, xml):
'''generate main header per XML file'''
f = open(os.path.join(directory, xml.basename + ".h"), mode='w')
t.write(f, '''
/** @file
* @brief MAVLink comm protocol generated from ${basename}.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef ${basename_upper}_H
#define ${basename_upper}_H
#ifdef __cplusplus
extern "C" {
#endif
// MESSAGE LENGTHS AND CRCS
#ifndef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS {${message_lengths_array}}
#endif
#ifndef MAVLINK_MESSAGE_CRCS
#define MAVLINK_MESSAGE_CRCS {${message_crcs_array}}
#endif
#ifndef MAVLINK_MESSAGE_INFO
#define MAVLINK_MESSAGE_INFO {${message_info_array}}
#endif
#include "../protocol.h"
#define MAVLINK_ENABLED_${basename_upper}
${{include_list:#include "../${base}/${base}.h"
}}
// MAVLINK VERSION
#ifndef MAVLINK_VERSION
#define MAVLINK_VERSION ${version}
#endif
#if (MAVLINK_VERSION == 0)
#undef MAVLINK_VERSION
#define MAVLINK_VERSION ${version}
#endif
// ENUM DEFINITIONS
${{enum:
/** @brief ${description} */
#ifndef HAVE_ENUM_${name}
#define HAVE_ENUM_${name}
enum ${name}
{
${{entry: ${name}=${value}, /* ${description} |${{param:${description}| }} */
}}
};
#endif
}}
// MESSAGE DEFINITIONS
${{message:#include "./mavlink_msg_${name_lower}.h"
}}
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // ${basename_upper}_H
''', xml)
f.close()
def generate_message_h(directory, m):
'''generate per-message header for a XML file'''
f = open(os.path.join(directory, 'mavlink_msg_%s.h' % m.name_lower), mode='w')
t.write(f, '''
// MESSAGE ${name} PACKING
#define MAVLINK_MSG_ID_${name} ${id}
typedef struct __mavlink_${name_lower}_t
{
${{ordered_fields: ${type} ${name}${array_suffix}; ///< ${description}
}}
} mavlink_${name_lower}_t;
#define MAVLINK_MSG_ID_${name}_LEN ${wire_length}
#define MAVLINK_MSG_ID_${id}_LEN ${wire_length}
${{array_fields:#define MAVLINK_MSG_${msg_name}_FIELD_${name_upper}_LEN ${array_length}
}}
#define MAVLINK_MESSAGE_INFO_${name} { \\
"${name}", \\
${num_fields}, \\
{ ${{ordered_fields: { "${name}", ${c_print_format}, MAVLINK_TYPE_${type_upper}, ${array_length}, ${wire_offset}, offsetof(mavlink_${name_lower}_t, ${name}) }, \\
}} } \\
}
/**
* @brief Pack a ${name_lower} message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
${{arg_fields: * @param ${name} ${description}
}}
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_${name_lower}_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[${wire_length}];
${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname});
}}
${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length});
#else
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length});
#endif
msg->msgid = MAVLINK_MSG_ID_${name};
return mavlink_finalize_message(msg, system_id, component_id, ${wire_length}${crc_extra_arg});
}
/**
* @brief Pack a ${name_lower} message on a channel
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
${{arg_fields: * @param ${name} ${description}
}}
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_${name_lower}_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
mavlink_message_t* msg,
${{arg_fields:${array_const}${type} ${array_prefix}${name},}})
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[${wire_length}];
${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname});
}}
${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, ${wire_length});
#else
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
}}
memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, ${wire_length});
#endif
msg->msgid = MAVLINK_MSG_ID_${name};
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, ${wire_length}${crc_extra_arg});
}
/**
* @brief Encode a ${name_lower} struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param ${name_lower} C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_${name_lower}_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
{
return mavlink_msg_${name_lower}_pack(system_id, component_id, msg,${{arg_fields: ${name_lower}->${name},}});
}
/**
* @brief Send a ${name_lower} message
* @param chan MAVLink channel to send the message
*
${{arg_fields: * @param ${name} ${description}
}}
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_${name_lower}_send(mavlink_channel_t chan,${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
char buf[${wire_length}];
${{scalar_fields: _mav_put_${type}(buf, ${wire_offset}, ${putname});
}}
${{array_fields: _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
}}
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, buf, ${wire_length}${crc_extra_arg});
#else
mavlink_${name_lower}_t packet;
${{scalar_fields: packet.${name} = ${putname};
}}
${{array_fields: mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
}}
_mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_${name}, (const char *)&packet, ${wire_length}${crc_extra_arg});
#endif
}
#endif
// MESSAGE ${name} UNPACKING
${{fields:
/**
* @brief Get field ${name} from ${name_lower} message
*
* @return ${description}
*/
static inline ${return_type} mavlink_msg_${name_lower}_get_${name}(const mavlink_message_t* msg${get_arg})
{
return _MAV_RETURN_${type}${array_tag}(msg, ${array_return_arg} ${wire_offset});
}
}}
/**
* @brief Decode a ${name_lower} message into a struct
*
* @param msg The message to decode
* @param ${name_lower} C-struct to decode the message contents into
*/
static inline void mavlink_msg_${name_lower}_decode(const mavlink_message_t* msg, mavlink_${name_lower}_t* ${name_lower})
{
#if MAVLINK_NEED_BYTE_SWAP
${{ordered_fields: ${decode_left}mavlink_msg_${name_lower}_get_${name}(msg${decode_right});
}}
#else
memcpy(${name_lower}, _MAV_PAYLOAD(msg), ${wire_length});
#endif
}
''', m)
f.close()
def generate_testsuite_h(directory, xml):
'''generate testsuite.h per XML file'''
f = open(os.path.join(directory, "testsuite.h"), mode='w')
t.write(f, '''
/** @file
* @brief MAVLink comm protocol testsuite generated from ${basename}.xml
* @see http://qgroundcontrol.org/mavlink/
*/
#ifndef ${basename_upper}_TESTSUITE_H
#define ${basename_upper}_TESTSUITE_H
#ifdef __cplusplus
extern "C" {
#endif
#ifndef MAVLINK_TEST_ALL
#define MAVLINK_TEST_ALL
${{include_list:static void mavlink_test_${base}(uint8_t, uint8_t, mavlink_message_t *last_msg);
}}
static void mavlink_test_${basename}(uint8_t, uint8_t, mavlink_message_t *last_msg);
static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
${{include_list: mavlink_test_${base}(system_id, component_id, last_msg);
}}
mavlink_test_${basename}(system_id, component_id, last_msg);
}
#endif
${{include_list:#include "../${base}/testsuite.h"
}}
${{message:
static void mavlink_test_${name_lower}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_${name_lower}_t packet_in = {
${{ordered_fields:${c_test_value},
}}};
mavlink_${name_lower}_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
${{scalar_fields: packet1.${name} = packet_in.${name};
}}
${{array_fields: mav_array_memcpy(packet1.${name}, packet_in.${name}, sizeof(${type})*${array_length});
}}
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_${name_lower}_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_${name_lower}_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_${name_lower}_pack(system_id, component_id, &msg ${{arg_fields:, packet1.${name} }});
mavlink_msg_${name_lower}_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_${name_lower}_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg ${{arg_fields:, packet1.${name} }});
mavlink_msg_${name_lower}_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_${name_lower}_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_${name_lower}_send(MAVLINK_COMM_1 ${{arg_fields:, packet1.${name} }});
mavlink_msg_${name_lower}_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
}}
static void mavlink_test_${basename}(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
${{message: mavlink_test_${name_lower}(system_id, component_id, last_msg);
}}
}
#ifdef __cplusplus
}
#endif // __cplusplus
#endif // ${basename_upper}_TESTSUITE_H
''', xml)
f.close()
def copy_fixed_headers(directory, xml):
'''copy the fixed protocol headers to the target directory'''
import shutil
hlist = [ 'protocol.h', 'mavlink_helpers.h', 'mavlink_types.h', 'checksum.h', 'mavlink_protobuf_manager.hpp' ]
basepath = os.path.dirname(os.path.realpath(__file__))
srcpath = os.path.join(basepath, 'C/include_v%s' % xml.wire_protocol_version)
print("Copying fixed headers")
for h in hlist:
if (not (h == 'mavlink_protobuf_manager.hpp' and xml.wire_protocol_version == '0.9')):
src = os.path.realpath(os.path.join(srcpath, h))
dest = os.path.realpath(os.path.join(directory, h))
if src == dest:
continue
shutil.copy(src, dest)
# XXX This is a hack - to be removed
if (xml.basename == 'pixhawk' and xml.wire_protocol_version == '1.0'):
h = 'pixhawk/pixhawk.pb.h'
src = os.path.realpath(os.path.join(srcpath, h))
dest = os.path.realpath(os.path.join(directory, h))
shutil.copy(src, dest)
def copy_fixed_sources(directory, xml):
# XXX This is a hack - to be removed
import shutil
basepath = os.path.dirname(os.path.realpath(__file__))
srcpath = os.path.join(basepath, 'C/src_v%s' % xml.wire_protocol_version)
if (xml.basename == 'pixhawk' and xml.wire_protocol_version == '1.0'):
print("Copying fixed sources")
src = os.path.realpath(os.path.join(srcpath, 'pixhawk/pixhawk.pb.cc'))
dest = os.path.realpath(os.path.join(directory, '../../../share/mavlink/src/v%s/pixhawk/pixhawk.pb.cc' % xml.wire_protocol_version))
destdir = os.path.realpath(os.path.join(directory, '../../../share/mavlink/src/v%s/pixhawk' % xml.wire_protocol_version))
try:
os.makedirs(destdir)
except:
print("Not re-creating directory")
shutil.copy(src, dest)
print("Copied to"),
print(dest)
class mav_include(object):
def __init__(self, base):
self.base = base
def generate_one(basename, xml):
'''generate headers for one XML file'''
directory = os.path.join(basename, xml.basename)
print("Generating C implementation in directory %s" % directory)
mavparse.mkdir_p(directory)
if xml.little_endian:
xml.mavlink_endian = "MAVLINK_LITTLE_ENDIAN"
else:
xml.mavlink_endian = "MAVLINK_BIG_ENDIAN"
if xml.crc_extra:
xml.crc_extra_define = "1"
else:
xml.crc_extra_define = "0"
if xml.sort_fields:
xml.aligned_fields_define = "1"
else:
xml.aligned_fields_define = "0"
# work out the included headers
xml.include_list = []
for i in xml.include:
base = i[:-4]
xml.include_list.append(mav_include(base))
# form message lengths array
xml.message_lengths_array = ''
for mlen in xml.message_lengths:
xml.message_lengths_array += '%u, ' % mlen
xml.message_lengths_array = xml.message_lengths_array[:-2]
# and message CRCs array
xml.message_crcs_array = ''
for crc in xml.message_crcs:
xml.message_crcs_array += '%u, ' % crc
xml.message_crcs_array = xml.message_crcs_array[:-2]
# form message info array
xml.message_info_array = ''
for name in xml.message_names:
if name is not None:
xml.message_info_array += 'MAVLINK_MESSAGE_INFO_%s, ' % name
else:
# Several C compilers don't accept {NULL} for
# multi-dimensional arrays and structs
# feed the compiler a "filled" empty message
xml.message_info_array += '{"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, '
xml.message_info_array = xml.message_info_array[:-2]
# add some extra field attributes for convenience with arrays
for m in xml.message:
m.msg_name = m.name
if xml.crc_extra:
m.crc_extra_arg = ", %s" % m.crc_extra
else:
m.crc_extra_arg = ""
for f in m.fields:
if f.print_format is None:
f.c_print_format = 'NULL'
else:
f.c_print_format = '"%s"' % f.print_format
if f.array_length != 0:
f.array_suffix = '[%u]' % f.array_length
f.array_prefix = '*'
f.array_tag = '_array'
f.array_arg = ', %u' % f.array_length
f.array_return_arg = '%s, %u, ' % (f.name, f.array_length)
f.array_const = 'const '
f.decode_left = ''
f.decode_right = ', %s->%s' % (m.name_lower, f.name)
f.return_type = 'uint16_t'
f.get_arg = ', %s *%s' % (f.type, f.name)
if f.type == 'char':
f.c_test_value = '"%s"' % f.test_value
else:
test_strings = []
for v in f.test_value:
test_strings.append(str(v))
f.c_test_value = '{ %s }' % ', '.join(test_strings)
else:
f.array_suffix = ''
f.array_prefix = ''
f.array_tag = ''
f.array_arg = ''
f.array_return_arg = ''
f.array_const = ''
f.decode_left = "%s->%s = " % (m.name_lower, f.name)
f.decode_right = ''
f.get_arg = ''
f.return_type = f.type
if f.type == 'char':
f.c_test_value = "'%s'" % f.test_value
elif f.type == 'uint64_t':
f.c_test_value = "%sULL" % f.test_value
elif f.type == 'int64_t':
f.c_test_value = "%sLL" % f.test_value
else:
f.c_test_value = f.test_value
# cope with uint8_t_mavlink_version
for m in xml.message:
m.arg_fields = []
m.array_fields = []
m.scalar_fields = []
for f in m.ordered_fields:
if f.array_length != 0:
m.array_fields.append(f)
else:
m.scalar_fields.append(f)
for f in m.fields:
if not f.omit_arg:
m.arg_fields.append(f)
f.putname = f.name
else:
f.putname = f.const_value
generate_mavlink_h(directory, xml)
generate_version_h(directory, xml)
generate_main_h(directory, xml)
for m in xml.message:
generate_message_h(directory, m)
generate_testsuite_h(directory, xml)
def generate(basename, xml_list):
'''generate complete MAVLink C implemenation'''
for xml in xml_list:
generate_one(basename, xml)
copy_fixed_headers(basename, xml_list[0])
copy_fixed_sources(basename, xml_list[0])
mavlink/share/pyshared/pymavlink/generator/mavgen_python.py
-455
View File
@@ -1,455 +0,0 @@
#!/usr/bin/env python
'''
parse a MAVLink protocol XML file and generate a python implementation
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
import sys, textwrap, os
import mavparse, mavtemplate
t = mavtemplate.MAVTemplate()
def generate_preamble(outf, msgs, args, xml):
print("Generating preamble")
t.write(outf, """
'''
MAVLink protocol implementation (auto-generated by mavgen.py)
Generated from: ${FILELIST}
Note: this file has been auto-generated. DO NOT EDIT
'''
import struct, array, mavutil, time
WIRE_PROTOCOL_VERSION = "${WIRE_PROTOCOL_VERSION}"
class MAVLink_header(object):
'''MAVLink message header'''
def __init__(self, msgId, mlen=0, seq=0, srcSystem=0, srcComponent=0):
self.mlen = mlen
self.seq = seq
self.srcSystem = srcSystem
self.srcComponent = srcComponent
self.msgId = msgId
def pack(self):
return struct.pack('BBBBBB', ${PROTOCOL_MARKER}, self.mlen, self.seq,
self.srcSystem, self.srcComponent, self.msgId)
class MAVLink_message(object):
'''base MAVLink message class'''
def __init__(self, msgId, name):
self._header = MAVLink_header(msgId)
self._payload = None
self._msgbuf = None
self._crc = None
self._fieldnames = []
self._type = name
def get_msgbuf(self):
return self._msgbuf
def get_header(self):
return self._header
def get_payload(self):
return self._payload
def get_crc(self):
return self._crc
def get_fieldnames(self):
return self._fieldnames
def get_type(self):
return self._type
def get_msgId(self):
return self._header.msgId
def get_srcSystem(self):
return self._header.srcSystem
def get_srcComponent(self):
return self._header.srcComponent
def get_seq(self):
return self._header.seq
def __str__(self):
ret = '%s {' % self._type
for a in self._fieldnames:
v = getattr(self, a)
ret += '%s : %s, ' % (a, v)
ret = ret[0:-2] + '}'
return ret
def pack(self, mav, crc_extra, payload):
self._payload = payload
self._header = MAVLink_header(self._header.msgId, len(payload), mav.seq,
mav.srcSystem, mav.srcComponent)
self._msgbuf = self._header.pack() + payload
crc = mavutil.x25crc(self._msgbuf[1:])
if ${crc_extra}: # using CRC extra
crc.accumulate(chr(crc_extra))
self._crc = crc.crc
self._msgbuf += struct.pack('<H', self._crc)
return self._msgbuf
""", {'FILELIST' : ",".join(args),
'PROTOCOL_MARKER' : xml.protocol_marker,
'crc_extra' : xml.crc_extra,
'WIRE_PROTOCOL_VERSION' : xml.wire_protocol_version })
def generate_enums(outf, enums):
print("Generating enums")
outf.write("\n# enums\n")
wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" # ")
for e in enums:
outf.write("\n# %s\n" % e.name)
for entry in e.entry:
outf.write("%s = %u # %s\n" % (entry.name, entry.value, wrapper.fill(entry.description)))
def generate_message_ids(outf, msgs):
print("Generating message IDs")
outf.write("\n# message IDs\n")
outf.write("MAVLINK_MSG_ID_BAD_DATA = -1\n")
for m in msgs:
outf.write("MAVLINK_MSG_ID_%s = %u\n" % (m.name.upper(), m.id))
def generate_classes(outf, msgs):
print("Generating class definitions")
wrapper = textwrap.TextWrapper(initial_indent=" ", subsequent_indent=" ")
for m in msgs:
outf.write("""
class MAVLink_%s_message(MAVLink_message):
'''
%s
'''
def __init__(self""" % (m.name.lower(), wrapper.fill(m.description.strip())))
if len(m.fields) != 0:
outf.write(", " + ", ".join(m.fieldnames))
outf.write("):\n")
outf.write(" MAVLink_message.__init__(self, MAVLINK_MSG_ID_%s, '%s')\n" % (m.name.upper(), m.name.upper()))
if len(m.fieldnames) != 0:
outf.write(" self._fieldnames = ['%s']\n" % "', '".join(m.fieldnames))
for f in m.fields:
outf.write(" self.%s = %s\n" % (f.name, f.name))
outf.write("""
def pack(self, mav):
return MAVLink_message.pack(self, mav, %u, struct.pack('%s'""" % (m.crc_extra, m.fmtstr))
if len(m.fields) != 0:
outf.write(", self." + ", self.".join(m.ordered_fieldnames))
outf.write("))\n")
def mavfmt(field):
'''work out the struct format for a type'''
map = {
'float' : 'f',
'double' : 'd',
'char' : 'c',
'int8_t' : 'b',
'uint8_t' : 'B',
'uint8_t_mavlink_version' : 'B',
'int16_t' : 'h',
'uint16_t' : 'H',
'int32_t' : 'i',
'uint32_t' : 'I',
'int64_t' : 'q',
'uint64_t' : 'Q',
}
if field.array_length:
if field.type in ['char', 'int8_t', 'uint8_t']:
return str(field.array_length)+'s'
return str(field.array_length)+map[field.type]
return map[field.type]
def generate_mavlink_class(outf, msgs, xml):
print("Generating MAVLink class")
outf.write("\n\nmavlink_map = {\n");
for m in msgs:
outf.write(" MAVLINK_MSG_ID_%s : ( '%s', MAVLink_%s_message, %s, %u ),\n" % (
m.name.upper(), m.fmtstr, m.name.lower(), m.order_map, m.crc_extra))
outf.write("}\n\n")
t.write(outf, """
class MAVError(Exception):
'''MAVLink error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = msg
class MAVString(str):
'''NUL terminated string'''
def __init__(self, s):
str.__init__(self)
def __str__(self):
i = self.find(chr(0))
if i == -1:
return self[:]
return self[0:i]
class MAVLink_bad_data(MAVLink_message):
'''
a piece of bad data in a mavlink stream
'''
def __init__(self, data, reason):
MAVLink_message.__init__(self, MAVLINK_MSG_ID_BAD_DATA, 'BAD_DATA')
self._fieldnames = ['data', 'reason']
self.data = data
self.reason = reason
self._msgbuf = data
class MAVLink(object):
'''MAVLink protocol handling class'''
def __init__(self, file, srcSystem=0, srcComponent=0):
self.seq = 0
self.file = file
self.srcSystem = srcSystem
self.srcComponent = srcComponent
self.callback = None
self.callback_args = None
self.callback_kwargs = None
self.buf = array.array('B')
self.expected_length = 6
self.have_prefix_error = False
self.robust_parsing = False
self.protocol_marker = ${protocol_marker}
self.little_endian = ${little_endian}
self.crc_extra = ${crc_extra}
self.sort_fields = ${sort_fields}
self.total_packets_sent = 0
self.total_bytes_sent = 0
self.total_packets_received = 0
self.total_bytes_received = 0
self.total_receive_errors = 0
self.startup_time = time.time()
def set_callback(self, callback, *args, **kwargs):
self.callback = callback
self.callback_args = args
self.callback_kwargs = kwargs
def send(self, mavmsg):
'''send a MAVLink message'''
buf = mavmsg.pack(self)
self.file.write(buf)
self.seq = (self.seq + 1) % 255
self.total_packets_sent += 1
self.total_bytes_sent += len(buf)
def bytes_needed(self):
'''return number of bytes needed for next parsing stage'''
ret = self.expected_length - len(self.buf)
if ret <= 0:
return 1
return ret
def parse_char(self, c):
'''input some data bytes, possibly returning a new message'''
if isinstance(c, str):
self.buf.fromstring(c)
else:
self.buf.extend(c)
self.total_bytes_received += len(c)
if len(self.buf) >= 1 and self.buf[0] != ${protocol_marker}:
magic = self.buf[0]
self.buf = self.buf[1:]
if self.robust_parsing:
m = MAVLink_bad_data(chr(magic), "Bad prefix")
if self.callback:
self.callback(m, *self.callback_args, **self.callback_kwargs)
self.expected_length = 6
self.total_receive_errors += 1
return m
if self.have_prefix_error:
return None
self.have_prefix_error = True
self.total_receive_errors += 1
raise MAVError("invalid MAVLink prefix '%s'" % magic)
self.have_prefix_error = False
if len(self.buf) >= 2:
(magic, self.expected_length) = struct.unpack('BB', self.buf[0:2])
self.expected_length += 8
if self.expected_length >= 8 and len(self.buf) >= self.expected_length:
mbuf = self.buf[0:self.expected_length]
self.buf = self.buf[self.expected_length:]
self.expected_length = 6
if self.robust_parsing:
try:
m = self.decode(mbuf)
self.total_packets_received += 1
except MAVError as reason:
m = MAVLink_bad_data(mbuf, reason.message)
self.total_receive_errors += 1
else:
m = self.decode(mbuf)
self.total_packets_received += 1
if self.callback:
self.callback(m, *self.callback_args, **self.callback_kwargs)
return m
return None
def parse_buffer(self, s):
'''input some data bytes, possibly returning a list of new messages'''
m = self.parse_char(s)
if m is None:
return None
ret = [m]
while True:
m = self.parse_char("")
if m is None:
return ret
ret.append(m)
return ret
def decode(self, msgbuf):
'''decode a buffer as a MAVLink message'''
# decode the header
try:
magic, mlen, seq, srcSystem, srcComponent, msgId = struct.unpack('cBBBBB', msgbuf[:6])
except struct.error as emsg:
raise MAVError('Unable to unpack MAVLink header: %s' % emsg)
if ord(magic) != ${protocol_marker}:
raise MAVError("invalid MAVLink prefix '%s'" % magic)
if mlen != len(msgbuf)-8:
raise MAVError('invalid MAVLink message length. Got %u expected %u, msgId=%u' % (len(msgbuf)-8, mlen, msgId))
if not msgId in mavlink_map:
raise MAVError('unknown MAVLink message ID %u' % msgId)
# decode the payload
(fmt, type, order_map, crc_extra) = mavlink_map[msgId]
# decode the checksum
try:
crc, = struct.unpack('<H', msgbuf[-2:])
except struct.error as emsg:
raise MAVError('Unable to unpack MAVLink CRC: %s' % emsg)
crc2 = mavutil.x25crc(msgbuf[1:-2])
if ${crc_extra}: # using CRC extra
crc2.accumulate(chr(crc_extra))
if crc != crc2.crc:
raise MAVError('invalid MAVLink CRC in msgID %u 0x%04x should be 0x%04x' % (msgId, crc, crc2.crc))
try:
t = struct.unpack(fmt, msgbuf[6:-2])
except struct.error as emsg:
raise MAVError('Unable to unpack MAVLink payload type=%s fmt=%s payloadLength=%u: %s' % (
type, fmt, len(msgbuf[6:-2]), emsg))
tlist = list(t)
# handle sorted fields
if ${sort_fields}:
t = tlist[:]
for i in range(0, len(tlist)):
tlist[i] = t[order_map[i]]
# terminate any strings
for i in range(0, len(tlist)):
if isinstance(tlist[i], str):
tlist[i] = MAVString(tlist[i])
t = tuple(tlist)
# construct the message object
try:
m = type(*t)
except Exception as emsg:
raise MAVError('Unable to instantiate MAVLink message of type %s : %s' % (type, emsg))
m._msgbuf = msgbuf
m._payload = msgbuf[6:-2]
m._crc = crc
m._header = MAVLink_header(msgId, mlen, seq, srcSystem, srcComponent)
return m
""", xml)
def generate_methods(outf, msgs):
print("Generating methods")
def field_descriptions(fields):
ret = ""
for f in fields:
ret += " %-18s : %s (%s)\n" % (f.name, f.description.strip(), f.type)
return ret
wrapper = textwrap.TextWrapper(initial_indent="", subsequent_indent=" ")
for m in msgs:
comment = "%s\n\n%s" % (wrapper.fill(m.description.strip()), field_descriptions(m.fields))
selffieldnames = 'self, '
for f in m.fields:
if f.omit_arg:
selffieldnames += '%s=%s, ' % (f.name, f.const_value)
else:
selffieldnames += '%s, ' % f.name
selffieldnames = selffieldnames[:-2]
sub = {'NAMELOWER' : m.name.lower(),
'SELFFIELDNAMES' : selffieldnames,
'COMMENT' : comment,
'FIELDNAMES' : ", ".join(m.fieldnames)}
t.write(outf, """
def ${NAMELOWER}_encode(${SELFFIELDNAMES}):
'''
${COMMENT}
'''
msg = MAVLink_${NAMELOWER}_message(${FIELDNAMES})
msg.pack(self)
return msg
""", sub)
t.write(outf, """
def ${NAMELOWER}_send(${SELFFIELDNAMES}):
'''
${COMMENT}
'''
return self.send(self.${NAMELOWER}_encode(${FIELDNAMES}))
""", sub)
def generate(basename, xml):
'''generate complete python implemenation'''
if basename.endswith('.py'):
filename = basename
else:
filename = basename + '.py'
msgs = []
enums = []
filelist = []
for x in xml:
msgs.extend(x.message)
enums.extend(x.enum)
filelist.append(os.path.basename(x.filename))
for m in msgs:
if xml[0].little_endian:
m.fmtstr = '<'
else:
m.fmtstr = '>'
for f in m.ordered_fields:
m.fmtstr += mavfmt(f)
m.order_map = [ 0 ] * len(m.fieldnames)
for i in range(0, len(m.fieldnames)):
m.order_map[i] = m.ordered_fieldnames.index(m.fieldnames[i])
print("Generating %s" % filename)
outf = open(filename, "w")
generate_preamble(outf, msgs, filelist, xml[0])
generate_enums(outf, enums)
generate_message_ids(outf, msgs)
generate_classes(outf, msgs)
generate_mavlink_class(outf, msgs, xml[0])
generate_methods(outf, msgs)
outf.close()
print("Generated %s OK" % filename)
mavlink/share/pyshared/pymavlink/generator/mavparse.py
-372
View File
@@ -1,372 +0,0 @@
#!/usr/bin/env python
'''
mavlink python parse functions
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
import xml.parsers.expat, os, errno, time, sys, operator, mavutil
PROTOCOL_0_9 = "0.9"
PROTOCOL_1_0 = "1.0"
class MAVParseError(Exception):
def __init__(self, message, inner_exception=None):
self.message = message
self.inner_exception = inner_exception
self.exception_info = sys.exc_info()
def __str__(self):
return self.message
class MAVField(object):
def __init__(self, name, type, print_format, xml, description=''):
self.name = name
self.name_upper = name.upper()
self.description = description
self.array_length = 0
self.omit_arg = False
self.const_value = None
self.print_format = print_format
lengths = {
'float' : 4,
'double' : 8,
'char' : 1,
'int8_t' : 1,
'uint8_t' : 1,
'uint8_t_mavlink_version' : 1,
'int16_t' : 2,
'uint16_t' : 2,
'int32_t' : 4,
'uint32_t' : 4,
'int64_t' : 8,
'uint64_t' : 8,
}
if type=='uint8_t_mavlink_version':
type = 'uint8_t'
self.omit_arg = True
self.const_value = xml.version
aidx = type.find("[")
if aidx != -1:
assert type[-1:] == ']'
self.array_length = int(type[aidx+1:-1])
type = type[0:aidx]
if type == 'array':
type = 'int8_t'
if type in lengths:
self.type_length = lengths[type]
self.type = type
elif (type+"_t") in lengths:
self.type_length = lengths[type+"_t"]
self.type = type+'_t'
else:
raise MAVParseError("unknown type '%s'" % type)
if self.array_length != 0:
self.wire_length = self.array_length * self.type_length
else:
self.wire_length = self.type_length
self.type_upper = self.type.upper()
def gen_test_value(self, i):
'''generate a testsuite value for a MAVField'''
if self.const_value:
return self.const_value
elif self.type == 'float':
return 17.0 + self.wire_offset*7 + i
elif self.type == 'double':
return 123.0 + self.wire_offset*7 + i
elif self.type == 'char':
return chr(ord('A') + (self.wire_offset + i)%26)
elif self.type in [ 'int8_t', 'uint8_t' ]:
return (5 + self.wire_offset*67 + i) & 0xFF
elif self.type in ['int16_t', 'uint16_t']:
return (17235 + self.wire_offset*52 + i) & 0xFFFF
elif self.type in ['int32_t', 'uint32_t']:
return (963497464 + self.wire_offset*52 + i)&0xFFFFFFFF
elif self.type in ['int64_t', 'uint64_t']:
return 93372036854775807 + self.wire_offset*63 + i
else:
raise MAVError('unknown type %s' % self.type)
def set_test_value(self):
'''set a testsuite value for a MAVField'''
if self.array_length:
self.test_value = []
for i in range(self.array_length):
self.test_value.append(self.gen_test_value(i))
else:
self.test_value = self.gen_test_value(0)
if self.type == 'char' and self.array_length:
v = ""
for c in self.test_value:
v += c
self.test_value = v[:-1]
class MAVType(object):
def __init__(self, name, id, linenumber, description=''):
self.name = name
self.name_lower = name.lower()
self.linenumber = linenumber
self.id = int(id)
self.description = description
self.fields = []
self.fieldnames = []
class MAVEnumParam(object):
def __init__(self, index, description=''):
self.index = index
self.description = description
class MAVEnumEntry(object):
def __init__(self, name, value, description='', end_marker=False):
self.name = name
self.value = value
self.description = description
self.param = []
self.end_marker = end_marker
class MAVEnum(object):
def __init__(self, name, linenumber, description=''):
self.name = name
self.description = description
self.entry = []
self.highest_value = 0
self.linenumber = linenumber
class MAVXML(object):
'''parse a mavlink XML file'''
def __init__(self, filename, wire_protocol_version=PROTOCOL_0_9):
self.filename = filename
self.basename = os.path.basename(filename)
if self.basename.lower().endswith(".xml"):
self.basename = self.basename[:-4]
self.basename_upper = self.basename.upper()
self.message = []
self.enum = []
self.parse_time = time.asctime()
self.version = 2
self.include = []
self.wire_protocol_version = wire_protocol_version
if wire_protocol_version == PROTOCOL_0_9:
self.protocol_marker = ord('U')
self.sort_fields = False
self.little_endian = False
self.crc_extra = False
elif wire_protocol_version == PROTOCOL_1_0:
self.protocol_marker = 0xFE
self.sort_fields = True
self.little_endian = True
self.crc_extra = True
else:
print("Unknown wire protocol version")
print("Available versions are: %s %s" % (PROTOCOL_0_9, PROTOCOL_1_0))
raise MAVParseError('Unknown MAVLink wire protocol version %s' % wire_protocol_version)
in_element_list = []
def check_attrs(attrs, check, where):
for c in check:
if not c in attrs:
raise MAVParseError('expected missing %s "%s" attribute at %s:%u' % (
where, c, filename, p.CurrentLineNumber))
def start_element(name, attrs):
in_element_list.append(name)
in_element = '.'.join(in_element_list)
#print in_element
if in_element == "mavlink.messages.message":
check_attrs(attrs, ['name', 'id'], 'message')
self.message.append(MAVType(attrs['name'], attrs['id'], p.CurrentLineNumber))
elif in_element == "mavlink.messages.message.field":
check_attrs(attrs, ['name', 'type'], 'field')
if 'print_format' in attrs:
print_format = attrs['print_format']
else:
print_format = None
self.message[-1].fields.append(MAVField(attrs['name'], attrs['type'],
print_format, self))
elif in_element == "mavlink.enums.enum":
check_attrs(attrs, ['name'], 'enum')
self.enum.append(MAVEnum(attrs['name'], p.CurrentLineNumber))
elif in_element == "mavlink.enums.enum.entry":
check_attrs(attrs, ['name'], 'enum entry')
if 'value' in attrs:
value = int(attrs['value'])
else:
value = self.enum[-1].highest_value + 1
if (value > self.enum[-1].highest_value):
self.enum[-1].highest_value = value
self.enum[-1].entry.append(MAVEnumEntry(attrs['name'], value))
elif in_element == "mavlink.enums.enum.entry.param":
check_attrs(attrs, ['index'], 'enum param')
self.enum[-1].entry[-1].param.append(MAVEnumParam(attrs['index']))
def end_element(name):
in_element = '.'.join(in_element_list)
if in_element == "mavlink.enums.enum":
# add a ENUM_END
self.enum[-1].entry.append(MAVEnumEntry("%s_ENUM_END" % self.enum[-1].name,
self.enum[-1].highest_value+1, end_marker=True))
in_element_list.pop()
def char_data(data):
in_element = '.'.join(in_element_list)
if in_element == "mavlink.messages.message.description":
self.message[-1].description += data
elif in_element == "mavlink.messages.message.field":
self.message[-1].fields[-1].description += data
elif in_element == "mavlink.enums.enum.description":
self.enum[-1].description += data
elif in_element == "mavlink.enums.enum.entry.description":
self.enum[-1].entry[-1].description += data
elif in_element == "mavlink.enums.enum.entry.param":
self.enum[-1].entry[-1].param[-1].description += data
elif in_element == "mavlink.version":
self.version = int(data)
elif in_element == "mavlink.include":
self.include.append(data)
f = open(filename, mode='rb')
p = xml.parsers.expat.ParserCreate()
p.StartElementHandler = start_element
p.EndElementHandler = end_element
p.CharacterDataHandler = char_data
p.ParseFile(f)
f.close()
self.message_lengths = [ 0 ] * 256
self.message_crcs = [ 0 ] * 256
self.message_names = [ None ] * 256
self.largest_payload = 0
for m in self.message:
m.wire_length = 0
m.fieldnames = []
m.ordered_fieldnames = []
if self.sort_fields:
m.ordered_fields = sorted(m.fields,
key=operator.attrgetter('type_length'),
reverse=True)
else:
m.ordered_fields = m.fields
for f in m.fields:
m.fieldnames.append(f.name)
for f in m.ordered_fields:
f.wire_offset = m.wire_length
m.wire_length += f.wire_length
m.ordered_fieldnames.append(f.name)
f.set_test_value()
m.num_fields = len(m.fieldnames)
if m.num_fields > 64:
raise MAVParseError("num_fields=%u : Maximum number of field names allowed is" % (
m.num_fields, 64))
m.crc_extra = message_checksum(m)
self.message_lengths[m.id] = m.wire_length
self.message_names[m.id] = m.name
self.message_crcs[m.id] = m.crc_extra
if m.wire_length > self.largest_payload:
self.largest_payload = m.wire_length
if m.wire_length+8 > 64:
print("Note: message %s is longer than 64 bytes long (%u bytes), which can cause fragmentation since many radio modems use 64 bytes as maximum air transfer unit." % (m.name, m.wire_length+8))
def __str__(self):
return "MAVXML for %s from %s (%u message, %u enums)" % (
self.basename, self.filename, len(self.message), len(self.enum))
def message_checksum(msg):
'''calculate a 8-bit checksum of the key fields of a message, so we
can detect incompatible XML changes'''
crc = mavutil.x25crc(msg.name + ' ')
for f in msg.ordered_fields:
crc.accumulate(f.type + ' ')
crc.accumulate(f.name + ' ')
if f.array_length:
crc.accumulate(chr(f.array_length))
return (crc.crc&0xFF) ^ (crc.crc>>8)
def merge_enums(xml):
'''merge enums between XML files'''
emap = {}
for x in xml:
newenums = []
for enum in x.enum:
if enum.name in emap:
emap[enum.name].entry.pop() # remove end marker
emap[enum.name].entry.extend(enum.entry)
print("Merged enum %s" % enum.name)
else:
newenums.append(enum)
emap[enum.name] = enum
x.enum = newenums
# sort by value
for e in emap:
emap[e].entry = sorted(emap[e].entry,
key=operator.attrgetter('value'),
reverse=False)
def check_duplicates(xml):
'''check for duplicate message IDs'''
merge_enums(xml)
msgmap = {}
enummap = {}
for x in xml:
for m in x.message:
if m.id in msgmap:
print("ERROR: Duplicate message id %u for %s (%s:%u) also used by %s" % (
m.id, m.name,
x.filename, m.linenumber,
msgmap[m.id]))
return True
fieldset = set()
for f in m.fields:
if f.name in fieldset:
print("ERROR: Duplicate field %s in message %s (%s:%u)" % (
f.name, m.name,
x.filename, m.linenumber))
return True
fieldset.add(f.name)
msgmap[m.id] = '%s (%s:%u)' % (m.name, x.filename, m.linenumber)
for enum in x.enum:
for entry in enum.entry:
s1 = "%s.%s" % (enum.name, entry.name)
s2 = "%s.%s" % (enum.name, entry.value)
if s1 in enummap or s2 in enummap:
print("ERROR: Duplicate enums %s/%s at %s:%u and %s" % (
s1, entry.value, x.filename, enum.linenumber,
enummap.get(s1) or enummap.get(s2)))
return True
enummap[s1] = "%s:%u" % (x.filename, enum.linenumber)
enummap[s2] = "%s:%u" % (x.filename, enum.linenumber)
return False
def total_msgs(xml):
'''count total number of msgs'''
count = 0
for x in xml:
count += len(x.message)
return count
def mkdir_p(dir):
try:
os.makedirs(dir)
except OSError as exc:
if exc.errno == errno.EEXIST:
pass
else: raise
# check version consistent
# add test.xml
# finish test suite
# printf style error macro, if defined call errors
mavlink/share/pyshared/pymavlink/generator/mavtemplate.py
-121
View File
@@ -1,121 +0,0 @@
#!/usr/bin/env python
'''
simple templating system for mavlink generator
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
from mavparse import MAVParseError
class MAVTemplate(object):
'''simple templating system'''
def __init__(self,
start_var_token="${",
end_var_token="}",
start_rep_token="${{",
end_rep_token="}}",
trim_leading_lf=True,
checkmissing=True):
self.start_var_token = start_var_token
self.end_var_token = end_var_token
self.start_rep_token = start_rep_token
self.end_rep_token = end_rep_token
self.trim_leading_lf = trim_leading_lf
self.checkmissing = checkmissing
def find_end(self, text, start_token, end_token):
'''find the of a token.
Returns the offset in the string immediately after the matching end_token'''
if not text.startswith(start_token):
raise MAVParseError("invalid token start")
offset = len(start_token)
nesting = 1
while nesting > 0:
idx1 = text[offset:].find(start_token)
idx2 = text[offset:].find(end_token)
if idx1 == -1 and idx2 == -1:
raise MAVParseError("token nesting error")
if idx1 == -1 or idx1 > idx2:
offset += idx2 + len(end_token)
nesting -= 1
else:
offset += idx1 + len(start_token)
nesting += 1
return offset
def find_var_end(self, text):
'''find the of a variable'''
return self.find_end(text, self.start_var_token, self.end_var_token)
def find_rep_end(self, text):
'''find the of a repitition'''
return self.find_end(text, self.start_rep_token, self.end_rep_token)
def substitute(self, text, subvars={},
trim_leading_lf=None, checkmissing=None):
'''substitute variables in a string'''
if trim_leading_lf is None:
trim_leading_lf = self.trim_leading_lf
if checkmissing is None:
checkmissing = self.checkmissing
# handle repititions
while True:
subidx = text.find(self.start_rep_token)
if subidx == -1:
break
endidx = self.find_rep_end(text[subidx:])
if endidx == -1:
raise MAVParseError("missing end macro in %s" % text[subidx:])
part1 = text[0:subidx]
part2 = text[subidx+len(self.start_rep_token):subidx+(endidx-len(self.end_rep_token))]
part3 = text[subidx+endidx:]
a = part2.split(':')
field_name = a[0]
rest = ':'.join(a[1:])
v = getattr(subvars, field_name, None)
if v is None:
raise MAVParseError('unable to find field %s' % field_name)
t1 = part1
for f in v:
t1 += self.substitute(rest, f, trim_leading_lf=False, checkmissing=False)
if len(v) != 0 and t1[-1] in ["\n", ","]:
t1 = t1[:-1]
t1 += part3
text = t1
if trim_leading_lf:
if text[0] == '\n':
text = text[1:]
while True:
idx = text.find(self.start_var_token)
if idx == -1:
return text
endidx = text[idx:].find(self.end_var_token)
if endidx == -1:
raise MAVParseError('missing end of variable: %s' % text[idx:idx+10])
varname = text[idx+2:idx+endidx]
if isinstance(subvars, dict):
if not varname in subvars:
if checkmissing:
raise MAVParseError("unknown variable in '%s%s%s'" % (
self.start_var_token, varname, self.end_var_token))
return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars,
trim_leading_lf=False, checkmissing=False)
value = subvars[varname]
else:
value = getattr(subvars, varname, None)
if value is None:
if checkmissing:
raise MAVParseError("unknown variable in '%s%s%s'" % (
self.start_var_token, varname, self.end_var_token))
return text[0:idx+endidx] + self.substitute(text[idx+endidx:], subvars,
trim_leading_lf=False, checkmissing=False)
text = text.replace("%s%s%s" % (self.start_var_token, varname, self.end_var_token), str(value))
return text
def write(self, file, text, subvars={}, trim_leading_lf=True):
'''write to a file with variable substitution'''
file.write(self.substitute(text, subvars=subvars, trim_leading_lf=trim_leading_lf))
mavlink/share/pyshared/pymavlink/generator/mavtestgen.py
-142
View File
@@ -1,142 +0,0 @@
#!/usr/bin/env python
'''
generate a MAVLink test suite
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
import sys, textwrap
from optparse import OptionParser
# mavparse is up a directory level
sys.path.append('..')
import mavparse
def gen_value(f, i, language):
'''generate a test value for the ith field of a message'''
type = f.type
# could be an array
if type.find("[") != -1:
aidx = type.find("[")
basetype = type[0:aidx]
if basetype == "array":
basetype = "int8_t"
if language == 'C':
return '(const %s *)"%s%u"' % (basetype, f.name, i)
return '"%s%u"' % (f.name, i)
if type == 'float':
return 17.0 + i*7
if type == 'char':
return 'A' + i
if type == 'int8_t':
return 5 + i
if type in ['int8_t', 'uint8_t']:
return 5 + i
if type in ['uint8_t_mavlink_version']:
return 2
if type in ['int16_t', 'uint16_t']:
return 17235 + i*52
if type in ['int32_t', 'uint32_t']:
v = 963497464 + i*52
if language == 'C':
return "%sL" % v
return v
if type in ['int64_t', 'uint64_t']:
v = 9223372036854775807 + i*63
if language == 'C':
return "%sLL" % v
return v
def generate_methods_python(outf, msgs):
outf.write("""
'''
MAVLink protocol test implementation (auto-generated by mavtestgen.py)
Generated from: %s
Note: this file has been auto-generated. DO NOT EDIT
'''
import mavlink
def generate_outputs(mav):
'''generate all message types as outputs'''
""")
for m in msgs:
if m.name == "HEARTBEAT": continue
outf.write("\tmav.%s_send(" % m.name.lower())
for i in range(0, len(m.fields)):
f = m.fields[i]
outf.write("%s=%s" % (f.name, gen_value(f, i, 'py')))
if i != len(m.fields)-1:
outf.write(",")
outf.write(")\n")
def generate_methods_C(outf, msgs):
outf.write("""
/*
MAVLink protocol test implementation (auto-generated by mavtestgen.py)
Generated from: %s
Note: this file has been auto-generated. DO NOT EDIT
*/
static void mavtest_generate_outputs(mavlink_channel_t chan)
{
""")
for m in msgs:
if m.name == "HEARTBEAT": continue
outf.write("\tmavlink_msg_%s_send(chan," % m.name.lower())
for i in range(0, len(m.fields)):
f = m.fields[i]
outf.write("%s" % gen_value(f, i, 'C'))
if i != len(m.fields)-1:
outf.write(",")
outf.write(");\n")
outf.write("}\n")
######################################################################
'''main program'''
parser = OptionParser("%prog [options] <XML files>")
parser.add_option("-o", "--output", dest="output", default="mavtest", help="output folder [default: %default]")
(opts, args) = parser.parse_args()
if len(args) < 1:
parser.error("You must supply at least one MAVLink XML protocol definition")
msgs = []
enums = []
for fname in args:
(m, e) = mavparse.parse_mavlink_xml(fname)
msgs.extend(m)
enums.extend(e)
if mavparse.check_duplicates(msgs):
sys.exit(1)
print("Found %u MAVLink message types" % len(msgs))
print("Generating python %s" % (opts.output+'.py'))
outf = open(opts.output + '.py', "w")
generate_methods_python(outf, msgs)
outf.close()
print("Generating C %s" % (opts.output+'.h'))
outf = open(opts.output + '.h', "w")
generate_methods_C(outf, msgs)
outf.close()
print("Generated %s OK" % opts.output)
mavlink/share/pyshared/pymavlink/mavextra.py
-154
View File
@@ -1,154 +0,0 @@
#!/usr/bin/env python
'''
useful extra functions for use by mavlink clients
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
from math import *
def kmh(mps):
'''convert m/s to Km/h'''
return mps*3.6
def altitude(press_abs, ground_press=955.0, ground_temp=30):
'''calculate barometric altitude'''
return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001
def mag_heading(RAW_IMU, ATTITUDE, declination=0, SENSOR_OFFSETS=None, ofs=None):
'''calculate heading from raw magnetometer'''
mag_x = RAW_IMU.xmag
mag_y = RAW_IMU.ymag
mag_z = RAW_IMU.zmag
if SENSOR_OFFSETS is not None and ofs is not None:
mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
headX = mag_x*cos(ATTITUDE.pitch) + mag_y*sin(ATTITUDE.roll)*sin(ATTITUDE.pitch) + mag_z*cos(ATTITUDE.roll)*sin(ATTITUDE.pitch)
headY = mag_y*cos(ATTITUDE.roll) - mag_z*sin(ATTITUDE.roll)
heading = degrees(atan2(-headY,headX)) + declination
if heading < 0:
heading += 360
return heading
def mag_field(RAW_IMU, SENSOR_OFFSETS=None, ofs=None):
'''calculate magnetic field strength from raw magnetometer'''
mag_x = RAW_IMU.xmag
mag_y = RAW_IMU.ymag
mag_z = RAW_IMU.zmag
if SENSOR_OFFSETS is not None and ofs is not None:
mag_x += ofs[0] - SENSOR_OFFSETS.mag_ofs_x
mag_y += ofs[1] - SENSOR_OFFSETS.mag_ofs_y
mag_z += ofs[2] - SENSOR_OFFSETS.mag_ofs_z
return sqrt(mag_x**2 + mag_y**2 + mag_z**2)
def angle_diff(angle1, angle2):
'''show the difference between two angles in degrees'''
ret = angle1 - angle2
if ret > 180:
ret -= 360;
if ret < -180:
ret += 360
return ret
lowpass_data = {}
def lowpass(var, key, factor):
'''a simple lowpass filter'''
global lowpass_data
if not key in lowpass_data:
lowpass_data[key] = var
else:
lowpass_data[key] = factor*lowpass_data[key] + (1.0 - factor)*var
return lowpass_data[key]
last_delta = {}
def delta(var, key):
'''calculate slope'''
global last_delta
dv = 0
if key in last_delta:
dv = var - last_delta[key]
last_delta[key] = var
return dv
def delta_angle(var, key):
'''calculate slope of an angle'''
global last_delta
dv = 0
if key in last_delta:
dv = var - last_delta[key]
last_delta[key] = var
if dv > 180:
dv -= 360
if dv < -180:
dv += 360
return dv
def roll_estimate(RAW_IMU,smooth=0.7):
'''estimate roll from accelerometer'''
rx = lowpass(RAW_IMU.xacc,'rx',smooth)
ry = lowpass(RAW_IMU.yacc,'ry',smooth)
rz = lowpass(RAW_IMU.zacc,'rz',smooth)
return degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2)))
def pitch_estimate(RAW_IMU, smooth=0.7):
'''estimate pitch from accelerometer'''
rx = lowpass(RAW_IMU.xacc,'rx',smooth)
ry = lowpass(RAW_IMU.yacc,'ry',smooth)
rz = lowpass(RAW_IMU.zacc,'rz',smooth)
return degrees(asin(rx/sqrt(rx**2+ry**2+rz**2)))
def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, smooth=0.7):
'''estimate pitch from accelerometer'''
rx = RAW_IMU.xacc
ry = RAW_IMU.yacc
rz = RAW_IMU.zacc+45
if SENSOR_OFFSETS is not None and ofs is not None:
rx += ofs[0] - SENSOR_OFFSETS.accel_cal_x
ry += ofs[1] - SENSOR_OFFSETS.accel_cal_y
rz += ofs[2] - SENSOR_OFFSETS.accel_cal_z
return lowpass(sqrt(rx**2+ry**2+rz**2)*0.01,'_gravity',smooth)
def pitch_sim(SIMSTATE, GPS_RAW):
'''estimate pitch from SIMSTATE accels'''
xacc = SIMSTATE.xacc - lowpass(delta(GPS_RAW.v,"v")*6.6, "v", 0.9)
zacc = SIMSTATE.zacc
zacc += SIMSTATE.ygyro * GPS_RAW.v;
if xacc/zacc >= 1:
return 0
if xacc/zacc <= -1:
return -0
return degrees(-asin(xacc/zacc))
def distance_two(GPS_RAW1, GPS_RAW2):
'''distance between two points'''
lat1 = radians(GPS_RAW1.lat)
lat2 = radians(GPS_RAW2.lat)
lon1 = radians(GPS_RAW1.lon)
lon2 = radians(GPS_RAW2.lon)
dLat = lat2 - lat1
dLon = lon2 - lon1
a = sin(0.5*dLat) * sin(0.5*dLat) + sin(0.5*dLon) * sin(0.5*dLon) * cos(lat1) * cos(lat2)
c = 2.0 * atan2(sqrt(a), sqrt(1.0-a))
return 6371 * 1000 * c
first_fix = None
def distance_home(GPS_RAW):
'''distance from first fix point'''
global first_fix
if first_fix == None or first_fix.fix_type < 2:
first_fix = GPS_RAW
return 0
return distance_two(GPS_RAW, first_fix)
mavlink/share/pyshared/pymavlink/mavlink.py
-4930
View File
File diff suppressed because it is too large Load Diff
mavlink/share/pyshared/pymavlink/mavlinkv10.py
-5394
View File
File diff suppressed because it is too large Load Diff
mavlink/share/pyshared/pymavlink/mavutil.py
-678
View File
@@ -1,678 +0,0 @@
#!/usr/bin/env python
'''
mavlink python utility functions
Copyright Andrew Tridgell 2011
Released under GNU GPL version 3 or later
'''
import socket, math, struct, time, os, fnmatch, array, sys, errno
from math import *
from mavextra import *
if os.getenv('MAVLINK10'):
import mavlinkv10 as mavlink
else:
import mavlink
def evaluate_expression(expression, vars):
'''evaluation an expression'''
try:
v = eval(expression, globals(), vars)
except NameError:
return None
return v
def evaluate_condition(condition, vars):
'''evaluation a conditional (boolean) statement'''
if condition is None:
return True
v = evaluate_expression(condition, vars)
if v is None:
return False
return v
class mavfile(object):
'''a generic mavlink port'''
def __init__(self, fd, address, source_system=255, notimestamps=False):
self.fd = fd
self.address = address
self.messages = { 'MAV' : self }
if mavlink.WIRE_PROTOCOL_VERSION == "1.0":
self.messages['HOME'] = mavlink.MAVLink_gps_raw_int_message(0,0,0,0,0,0,0,0,0,0)
mavlink.MAVLink_waypoint_message = mavlink.MAVLink_mission_item_message
else:
self.messages['HOME'] = mavlink.MAVLink_gps_raw_message(0,0,0,0,0,0,0,0,0)
self.params = {}
self.mav = None
self.target_system = 0
self.target_component = 0
self.mav = mavlink.MAVLink(self, srcSystem=source_system)
self.mav.robust_parsing = True
self.logfile = None
self.logfile_raw = None
self.param_fetch_in_progress = False
self.param_fetch_complete = False
self.start_time = time.time()
self.flightmode = "UNKNOWN"
self.timestamp = 0
self.message_hooks = []
self.idle_hooks = []
self.usec = 0
self.notimestamps = notimestamps
self._timestamp = None
def recv(self, n=None):
'''default recv method'''
raise RuntimeError('no recv() method supplied')
def close(self, n=None):
'''default close method'''
raise RuntimeError('no close() method supplied')
def write(self, buf):
'''default write method'''
raise RuntimeError('no write() method supplied')
def pre_message(self):
'''default pre message call'''
return
def post_message(self, msg):
'''default post message call'''
msg._timestamp = time.time()
type = msg.get_type()
self.messages[type] = msg
if self._timestamp is not None:
if self.notimestamps:
if 'usec' in msg.__dict__:
self.usec = msg.usec / 1.0e6
msg._timestamp = self.usec
else:
msg._timestamp = self._timestamp
self.timestamp = msg._timestamp
if type == 'HEARTBEAT':
self.target_system = msg.get_srcSystem()
self.target_component = msg.get_srcComponent()
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.flightmode = mode_string_v10(msg)
elif type == 'PARAM_VALUE':
self.params[str(msg.param_id)] = msg.param_value
if msg.param_index+1 == msg.param_count:
self.param_fetch_in_progress = False
self.param_fetch_complete = True
elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9':
self.flightmode = mode_string_v09(msg)
elif type == 'GPS_RAW':
if self.messages['HOME'].fix_type < 2:
self.messages['HOME'] = msg
for hook in self.message_hooks:
hook(self, msg)
def recv_msg(self):
'''message receive routine'''
self.pre_message()
while True:
n = self.mav.bytes_needed()
s = self.recv(n)
if len(s) == 0 and len(self.mav.buf) == 0:
return None
if self.logfile_raw:
self.logfile_raw.write(str(s))
msg = self.mav.parse_char(s)
if msg:
self.post_message(msg)
return msg
def recv_match(self, condition=None, type=None, blocking=False):
'''recv the next MAVLink message that matches the given condition'''
while True:
m = self.recv_msg()
if m is None:
if blocking:
for hook in self.idle_hooks:
hook(self)
time.sleep(0.01)
continue
return None
if type is not None and type != m.get_type():
continue
if not evaluate_condition(condition, self.messages):
continue
return m
def setup_logfile(self, logfile, mode='w'):
'''start logging to the given logfile, with timestamps'''
self.logfile = open(logfile, mode=mode)
def setup_logfile_raw(self, logfile, mode='w'):
'''start logging raw bytes to the given logfile, without timestamps'''
self.logfile_raw = open(logfile, mode=mode)
def wait_heartbeat(self, blocking=True):
'''wait for a heartbeat so we know the target system IDs'''
return self.recv_match(type='HEARTBEAT', blocking=blocking)
def param_fetch_all(self):
'''initiate fetch of all parameters'''
if time.time() - getattr(self, 'param_fetch_start', 0) < 2.0:
# don't fetch too often
return
self.param_fetch_start = time.time()
self.param_fetch_in_progress = True
self.mav.param_request_list_send(self.target_system, self.target_component)
def time_since(self, mtype):
'''return the time since the last message of type mtype was received'''
if not mtype in self.messages:
return time.time() - self.start_time
return time.time() - self.messages[mtype]._timestamp
def param_set_send(self, parm_name, parm_value, parm_type=None):
'''wrapper for parameter set'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
if parm_type == None:
parm_type = mavlink.MAV_VAR_FLOAT
self.mav.param_set_send(self.target_system, self.target_component,
parm_name, parm_value, parm_type)
else:
self.mav.param_set_send(self.target_system, self.target_component,
parm_name, parm_value)
def waypoint_request_list_send(self):
'''wrapper for waypoint_request_list_send'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.mav.mission_request_list_send(self.target_system, self.target_component)
else:
self.mav.waypoint_request_list_send(self.target_system, self.target_component)
def waypoint_clear_all_send(self):
'''wrapper for waypoint_clear_all_send'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.mav.mission_clear_all_send(self.target_system, self.target_component)
else:
self.mav.waypoint_clear_all_send(self.target_system, self.target_component)
def waypoint_request_send(self, seq):
'''wrapper for waypoint_request_send'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.mav.mission_request_send(self.target_system, self.target_component, seq)
else:
self.mav.waypoint_request_send(self.target_system, self.target_component, seq)
def waypoint_set_current_send(self, seq):
'''wrapper for waypoint_set_current_send'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.mav.mission_set_current_send(self.target_system, self.target_component, seq)
else:
self.mav.waypoint_set_current_send(self.target_system, self.target_component, seq)
def waypoint_count_send(self, seq):
'''wrapper for waypoint_count_send'''
if mavlink.WIRE_PROTOCOL_VERSION == '1.0':
self.mav.mission_count_send(self.target_system, self.target_component, seq)
else:
self.mav.waypoint_count_send(self.target_system, self.target_component, seq)
class mavserial(mavfile):
'''a serial mavlink port'''
def __init__(self, device, baud=115200, autoreconnect=False, source_system=255):
import serial
self.baud = baud
self.device = device
self.autoreconnect = autoreconnect
self.port = serial.Serial(self.device, self.baud, timeout=0,
dsrdtr=False, rtscts=False, xonxoff=False)
try:
fd = self.port.fileno()
except Exception:
fd = None
mavfile.__init__(self, fd, device, source_system=source_system)
def close(self):
self.port.close()
def recv(self,n=None):
if n is None:
n = self.mav.bytes_needed()
if self.fd is None:
waiting = self.port.inWaiting()
if waiting < n:
n = waiting
return self.port.read(n)
def write(self, buf):
try:
return self.port.write(buf)
except OSError:
if self.autoreconnect:
self.reset()
return -1
def reset(self):
import serial
self.port.close()
while True:
try:
self.port = serial.Serial(self.device, self.baud, timeout=1,
dsrdtr=False, rtscts=False, xonxoff=False)
try:
self.fd = self.port.fileno()
except Exception:
self.fd = None
return
except Exception:
print("Failed to reopen %s" % self.device)
time.sleep(1)
class mavudp(mavfile):
'''a UDP mavlink socket'''
def __init__(self, device, input=True, source_system=255):
a = device.split(':')
if len(a) != 2:
print("UDP ports must be specified as host:port")
sys.exit(1)
self.port = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.udp_server = input
if input:
self.port.bind((a[0], int(a[1])))
else:
self.destination_addr = (a[0], int(a[1]))
self.port.setblocking(0)
self.last_address = None
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
def close(self):
self.port.close()
def recv(self,n=None):
try:
data, self.last_address = self.port.recvfrom(300)
except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
return ""
raise
return data
def write(self, buf):
try:
if self.udp_server:
if self.last_address:
self.port.sendto(buf, self.last_address)
else:
self.port.sendto(buf, self.destination_addr)
except socket.error:
pass
def recv_msg(self):
'''message receive routine for UDP link'''
self.pre_message()
s = self.recv()
if len(s) == 0:
return None
msg = self.mav.parse_buffer(s)
if msg is not None:
for m in msg:
self.post_message(m)
return msg[0]
return None
class mavtcp(mavfile):
'''a TCP mavlink socket'''
def __init__(self, device, source_system=255):
a = device.split(':')
if len(a) != 2:
print("TCP ports must be specified as host:port")
sys.exit(1)
self.port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.destination_addr = (a[0], int(a[1]))
self.port.connect(self.destination_addr)
self.port.setblocking(0)
self.port.setsockopt(socket.SOL_TCP, socket.TCP_NODELAY, 1)
mavfile.__init__(self, self.port.fileno(), device, source_system=source_system)
def close(self):
self.port.close()
def recv(self,n=None):
if n is None:
n = self.mav.bytes_needed()
try:
data = self.port.recv(n)
except socket.error as e:
if e.errno in [ errno.EAGAIN, errno.EWOULDBLOCK ]:
return ""
raise
return data
def write(self, buf):
try:
self.port.send(buf)
except socket.error:
pass
class mavlogfile(mavfile):
'''a MAVLink logfile reader/writer'''
def __init__(self, filename, planner_format=None,
write=False, append=False,
robust_parsing=True, notimestamps=False, source_system=255):
self.filename = filename
self.writeable = write
self.robust_parsing = robust_parsing
self.planner_format = planner_format
self._two64 = math.pow(2.0, 63)
mode = 'rb'
if self.writeable:
if append:
mode = 'ab'
else:
mode = 'wb'
self.f = open(filename, mode)
self.filesize = os.path.getsize(filename)
self.percent = 0
mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps)
if self.notimestamps:
self._timestamp = 0
else:
self._timestamp = time.time()
def close(self):
self.f.close()
def recv(self,n=None):
if n is None:
n = self.mav.bytes_needed()
return self.f.read(n)
def write(self, buf):
self.f.write(buf)
def pre_message(self):
'''read timestamp if needed'''
# read the timestamp
self.percent = (100.0 * self.f.tell()) / self.filesize
if self.notimestamps:
return
if self.planner_format:
tbuf = self.f.read(21)
if len(tbuf) != 21 or tbuf[0] != '-' or tbuf[20] != ':':
raise RuntimeError('bad planner timestamp %s' % tbuf)
hnsec = self._two64 + float(tbuf[0:20])
t = hnsec * 1.0e-7 # convert to seconds
t -= 719163 * 24 * 60 * 60 # convert to 1970 base
else:
tbuf = self.f.read(8)
if len(tbuf) != 8:
return
(tusec,) = struct.unpack('>Q', tbuf)
t = tusec * 1.0e-6
self._timestamp = t
def post_message(self, msg):
'''add timestamp to message'''
# read the timestamp
super(mavlogfile, self).post_message(msg)
if self.planner_format:
self.f.read(1) # trailing newline
self.timestamp = msg._timestamp
class mavchildexec(mavfile):
'''a MAVLink child processes reader/writer'''
def __init__(self, filename, source_system=255):
from subprocess import Popen, PIPE
import fcntl
self.filename = filename
self.child = Popen(filename, shell=True, stdout=PIPE, stdin=PIPE)
self.fd = self.child.stdout.fileno()
fl = fcntl.fcntl(self.fd, fcntl.F_GETFL)
fcntl.fcntl(self.fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
fl = fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_GETFL)
fcntl.fcntl(self.child.stdout.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
mavfile.__init__(self, self.fd, filename, source_system=source_system)
def close(self):
self.child.close()
def recv(self,n=None):
try:
x = self.child.stdout.read(1)
except Exception:
return ''
return x
def write(self, buf):
self.child.stdin.write(buf)
def mavlink_connection(device, baud=115200, source_system=255,
planner_format=None, write=False, append=False,
robust_parsing=True, notimestamps=False, input=True):
'''make a serial or UDP mavlink connection'''
if device.startswith('tcp:'):
return mavtcp(device[4:], source_system=source_system)
if device.startswith('udp:'):
return mavudp(device[4:], input=input, source_system=source_system)
if device.find(':') != -1 and not device.endswith('log'):
return mavudp(device, source_system=source_system, input=input)
if os.path.isfile(device):
if device.endswith(".elf"):
return mavchildexec(device, source_system=source_system)
else:
return mavlogfile(device, planner_format=planner_format, write=write,
append=append, robust_parsing=robust_parsing, notimestamps=notimestamps,
source_system=source_system)
return mavserial(device, baud=baud, source_system=source_system)
class periodic_event(object):
'''a class for fixed frequency events'''
def __init__(self, frequency):
self.frequency = float(frequency)
self.last_time = time.time()
def force(self):
'''force immediate triggering'''
self.last_time = 0
def trigger(self):
'''return True if we should trigger now'''
tnow = time.time()
if self.last_time + (1.0/self.frequency) <= tnow:
self.last_time = tnow
return True
return False
try:
from curses import ascii
have_ascii = True
except:
have_ascii = False
def is_printable(c):
'''see if a character is printable'''
global have_ascii
if have_ascii:
return ascii.isprint(c)
if isinstance(c, int):
ic = c
else:
ic = ord(c)
return ic >= 32 and ic <= 126
def all_printable(buf):
'''see if a string is all printable'''
for c in buf:
if not is_printable(c) and not c in ['\r', '\n', '\t']:
return False
return True
class SerialPort(object):
'''auto-detected serial port'''
def __init__(self, device, description=None, hwid=None):
self.device = device
self.description = description
self.hwid = hwid
def __str__(self):
ret = self.device
if self.description is not None:
ret += " : " + self.description
if self.hwid is not None:
ret += " : " + self.hwid
return ret
def auto_detect_serial_win32(preferred_list=['*']):
'''try to auto-detect serial ports on win32'''
try:
import scanwin32
list = sorted(scanwin32.comports())
except:
return []
ret = []
for order, port, desc, hwid in list:
for preferred in preferred_list:
if fnmatch.fnmatch(desc, preferred) or fnmatch.fnmatch(hwid, preferred):
ret.append(SerialPort(port, description=desc, hwid=hwid))
if len(ret) > 0:
return ret
# now the rest
for order, port, desc, hwid in list:
ret.append(SerialPort(port, description=desc, hwid=hwid))
return ret
def auto_detect_serial_unix(preferred_list=['*']):
'''try to auto-detect serial ports on win32'''
import glob
glist = glob.glob('/dev/ttyS*') + glob.glob('/dev/ttyUSB*') + glob.glob('/dev/ttyACM*') + glob.glob('/dev/serial/by-id/*')
ret = []
# try preferred ones first
for d in glist:
for preferred in preferred_list:
if fnmatch.fnmatch(d, preferred):
ret.append(SerialPort(d))
if len(ret) > 0:
return ret
# now the rest
for d in glist:
ret.append(SerialPort(d))
return ret
def auto_detect_serial(preferred_list=['*']):
'''try to auto-detect serial port'''
# see if
if os.name == 'nt':
return auto_detect_serial_win32(preferred_list=preferred_list)
return auto_detect_serial_unix(preferred_list=preferred_list)
def mode_string_v09(msg):
'''mode string for 0.9 protocol'''
mode = msg.mode
nav_mode = msg.nav_mode
MAV_MODE_UNINIT = 0
MAV_MODE_MANUAL = 2
MAV_MODE_GUIDED = 3
MAV_MODE_AUTO = 4
MAV_MODE_TEST1 = 5
MAV_MODE_TEST2 = 6
MAV_MODE_TEST3 = 7
MAV_NAV_GROUNDED = 0
MAV_NAV_LIFTOFF = 1
MAV_NAV_HOLD = 2
MAV_NAV_WAYPOINT = 3
MAV_NAV_VECTOR = 4
MAV_NAV_RETURNING = 5
MAV_NAV_LANDING = 6
MAV_NAV_LOST = 7
MAV_NAV_LOITER = 8
cmode = (mode, nav_mode)
mapping = {
(MAV_MODE_UNINIT, MAV_NAV_GROUNDED) : "INITIALISING",
(MAV_MODE_MANUAL, MAV_NAV_VECTOR) : "MANUAL",
(MAV_MODE_TEST3, MAV_NAV_VECTOR) : "CIRCLE",
(MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
(MAV_MODE_TEST1, MAV_NAV_VECTOR) : "STABILIZE",
(MAV_MODE_TEST2, MAV_NAV_LIFTOFF) : "FBWA",
(MAV_MODE_AUTO, MAV_NAV_WAYPOINT) : "AUTO",
(MAV_MODE_AUTO, MAV_NAV_RETURNING) : "RTL",
(MAV_MODE_AUTO, MAV_NAV_LOITER) : "LOITER",
(MAV_MODE_AUTO, MAV_NAV_LIFTOFF) : "TAKEOFF",
(MAV_MODE_AUTO, MAV_NAV_LANDING) : "LANDING",
(MAV_MODE_AUTO, MAV_NAV_HOLD) : "LOITER",
(MAV_MODE_GUIDED, MAV_NAV_VECTOR) : "GUIDED",
(MAV_MODE_GUIDED, MAV_NAV_WAYPOINT) : "GUIDED",
(100, MAV_NAV_VECTOR) : "STABILIZE",
(101, MAV_NAV_VECTOR) : "ACRO",
(102, MAV_NAV_VECTOR) : "ALT_HOLD",
(107, MAV_NAV_VECTOR) : "CIRCLE",
(109, MAV_NAV_VECTOR) : "LAND",
}
if cmode in mapping:
return mapping[cmode]
return "Mode(%s,%s)" % cmode
def mode_string_v10(msg):
'''mode string for 1.0 protocol, from heartbeat'''
if not msg.base_mode & mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED:
return "Mode(0x%08x)" % msg.base_mode
mapping = {
0 : 'MANUAL',
1 : 'CIRCLE',
2 : 'STABILIZE',
5 : 'FBWA',
6 : 'FBWB',
7 : 'FBWC',
10 : 'AUTO',
11 : 'RTL',
12 : 'LOITER',
13 : 'TAKEOFF',
14 : 'LAND',
15 : 'GUIDED',
16 : 'INITIALISING'
}
if msg.custom_mode in mapping:
return mapping[msg.custom_mode]
return "Mode(%u)" % msg.custom_mode
class x25crc(object):
'''x25 CRC - based on checksum.h from mavlink library'''
def __init__(self, buf=''):
self.crc = 0xffff
self.accumulate(buf)
def accumulate(self, buf):
'''add in some more bytes'''
bytes = array.array('B')
if isinstance(buf, array.array):
bytes.extend(buf)
else:
bytes.fromstring(buf)
accum = self.crc
for b in bytes:
tmp = b ^ (accum & 0xff)
tmp = (tmp ^ (tmp<<4)) & 0xFF
accum = (accum>>8) ^ (tmp<<8) ^ (tmp<<3) ^ (tmp>>4)
accum = accum & 0xFFFF
self.crc = accum
mavlink/share/pyshared/pymavlink/mavwp.py
-200
View File
@@ -1,200 +0,0 @@
'''
module for loading/saving waypoints
'''
import os
if os.getenv('MAVLINK10'):
import mavlinkv10 as mavlink
else:
import mavlink
class MAVWPError(Exception):
'''MAVLink WP error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = msg
class MAVWPLoader(object):
'''MAVLink waypoint loader'''
def __init__(self, target_system=0, target_component=0):
self.wpoints = []
self.target_system = target_system
self.target_component = target_component
def count(self):
'''return number of waypoints'''
return len(self.wpoints)
def wp(self, i):
'''return a waypoint'''
return self.wpoints[i]
def add(self, w):
'''add a waypoint'''
w.seq = self.count()
self.wpoints.append(w)
def remove(self, w):
'''remove a waypoint'''
self.wpoints.remove(w)
def clear(self):
'''clear waypoint list'''
self.wpoints = []
def _read_waypoint_v100(self, line):
'''read a version 100 waypoint'''
cmdmap = {
2 : mavlink.MAV_CMD_NAV_TAKEOFF,
3 : mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH,
4 : mavlink.MAV_CMD_NAV_LAND,
24: mavlink.MAV_CMD_NAV_TAKEOFF,
26: mavlink.MAV_CMD_NAV_LAND,
25: mavlink.MAV_CMD_NAV_WAYPOINT ,
27: mavlink.MAV_CMD_NAV_LOITER_UNLIM
}
a = line.split()
if len(a) != 13:
raise MAVWPError("invalid waypoint line with %u values" % len(a))
w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
int(a[0]), # seq
int(a[1]), # frame
int(a[2]), # action
int(a[7]), # current
int(a[12]), # autocontinue
float(a[5]), # param1,
float(a[6]), # param2,
float(a[3]), # param3
float(a[4]), # param4
float(a[9]), # x, latitude
float(a[8]), # y, longitude
float(a[10]) # z
)
if not w.command in cmdmap:
raise MAVWPError("Unknown v100 waypoint action %u" % w.command)
w.command = cmdmap[w.command]
return w
def _read_waypoint_v110(self, line):
'''read a version 110 waypoint'''
a = line.split()
if len(a) != 12:
raise MAVWPError("invalid waypoint line with %u values" % len(a))
w = mavlink.MAVLink_waypoint_message(self.target_system, self.target_component,
int(a[0]), # seq
int(a[2]), # frame
int(a[3]), # command
int(a[1]), # current
int(a[11]), # autocontinue
float(a[4]), # param1,
float(a[5]), # param2,
float(a[6]), # param3
float(a[7]), # param4
float(a[8]), # x (latitude)
float(a[9]), # y (longitude)
float(a[10]) # z (altitude)
)
return w
def load(self, filename):
'''load waypoints from a file.
returns number of waypoints loaded'''
f = open(filename, mode='r')
version_line = f.readline().strip()
if version_line == "QGC WPL 100":
readfn = self._read_waypoint_v100
elif version_line == "QGC WPL 110":
readfn = self._read_waypoint_v110
else:
f.close()
raise MAVWPError("Unsupported waypoint format '%s'" % version_line)
self.clear()
for line in f:
if line.startswith('#'):
continue
line = line.strip()
if not line:
continue
w = readfn(line)
if w is not None:
self.add(w)
f.close()
return len(self.wpoints)
def save(self, filename):
'''save waypoints to a file'''
f = open(filename, mode='w')
f.write("QGC WPL 110\n")
for w in self.wpoints:
f.write("%u\t%u\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\t%u\n" % (
w.seq, w.current, w.frame, w.command,
w.param1, w.param2, w.param3, w.param4,
w.x, w.y, w.z, w.autocontinue))
f.close()
class MAVFenceError(Exception):
'''MAVLink fence error class'''
def __init__(self, msg):
Exception.__init__(self, msg)
self.message = msg
class MAVFenceLoader(object):
'''MAVLink geo-fence loader'''
def __init__(self, target_system=0, target_component=0):
self.points = []
self.target_system = target_system
self.target_component = target_component
def count(self):
'''return number of points'''
return len(self.points)
def point(self, i):
'''return a point'''
return self.points[i]
def add(self, p):
'''add a point'''
self.points.append(p)
def clear(self):
'''clear point list'''
self.points = []
def load(self, filename):
'''load points from a file.
returns number of points loaded'''
f = open(filename, mode='r')
self.clear()
for line in f:
if line.startswith('#'):
continue
line = line.strip()
if not line:
continue
a = line.split()
if len(a) != 2:
raise MAVFenceError("invalid fence point line: %s" % line)
p = mavlink.MAVLink_fence_point_message(self.target_system, self.target_component,
self.count(), 0, float(a[0]), float(a[1]))
self.add(p)
f.close()
for i in range(self.count()):
self.points[i].count = self.count()
return len(self.points)
def save(self, filename):
'''save fence points to a file'''
f = open(filename, mode='w')
for p in self.points:
f.write("%f\t%f\n" % (p.lat, p.lng))
f.close()
mavlink/share/pyshared/pymavlink/scanwin32.py
-236
View File
@@ -1,236 +0,0 @@
#!/usr/bin/env python
# this is taken from the pySerial documentation at
# http://pyserial.sourceforge.net/examples.html
import ctypes
import re
def ValidHandle(value):
if value == 0:
raise ctypes.WinError()
return value
NULL = 0
HDEVINFO = ctypes.c_int
BOOL = ctypes.c_int
CHAR = ctypes.c_char
PCTSTR = ctypes.c_char_p
HWND = ctypes.c_uint
DWORD = ctypes.c_ulong
PDWORD = ctypes.POINTER(DWORD)
ULONG = ctypes.c_ulong
ULONG_PTR = ctypes.POINTER(ULONG)
#~ PBYTE = ctypes.c_char_p
PBYTE = ctypes.c_void_p
class GUID(ctypes.Structure):
_fields_ = [
('Data1', ctypes.c_ulong),
('Data2', ctypes.c_ushort),
('Data3', ctypes.c_ushort),
('Data4', ctypes.c_ubyte*8),
]
def __str__(self):
return "{%08x-%04x-%04x-%s-%s}" % (
self.Data1,
self.Data2,
self.Data3,
''.join(["%02x" % d for d in self.Data4[:2]]),
''.join(["%02x" % d for d in self.Data4[2:]]),
)
class SP_DEVINFO_DATA(ctypes.Structure):
_fields_ = [
('cbSize', DWORD),
('ClassGuid', GUID),
('DevInst', DWORD),
('Reserved', ULONG_PTR),
]
def __str__(self):
return "ClassGuid:%s DevInst:%s" % (self.ClassGuid, self.DevInst)
PSP_DEVINFO_DATA = ctypes.POINTER(SP_DEVINFO_DATA)
class SP_DEVICE_INTERFACE_DATA(ctypes.Structure):
_fields_ = [
('cbSize', DWORD),
('InterfaceClassGuid', GUID),
('Flags', DWORD),
('Reserved', ULONG_PTR),
]
def __str__(self):
return "InterfaceClassGuid:%s Flags:%s" % (self.InterfaceClassGuid, self.Flags)
PSP_DEVICE_INTERFACE_DATA = ctypes.POINTER(SP_DEVICE_INTERFACE_DATA)
PSP_DEVICE_INTERFACE_DETAIL_DATA = ctypes.c_void_p
class dummy(ctypes.Structure):
_fields_=[("d1", DWORD), ("d2", CHAR)]
_pack_ = 1
SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A = ctypes.sizeof(dummy)
SetupDiDestroyDeviceInfoList = ctypes.windll.setupapi.SetupDiDestroyDeviceInfoList
SetupDiDestroyDeviceInfoList.argtypes = [HDEVINFO]
SetupDiDestroyDeviceInfoList.restype = BOOL
SetupDiGetClassDevs = ctypes.windll.setupapi.SetupDiGetClassDevsA
SetupDiGetClassDevs.argtypes = [ctypes.POINTER(GUID), PCTSTR, HWND, DWORD]
SetupDiGetClassDevs.restype = ValidHandle # HDEVINFO
SetupDiEnumDeviceInterfaces = ctypes.windll.setupapi.SetupDiEnumDeviceInterfaces
SetupDiEnumDeviceInterfaces.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, ctypes.POINTER(GUID), DWORD, PSP_DEVICE_INTERFACE_DATA]
SetupDiEnumDeviceInterfaces.restype = BOOL
SetupDiGetDeviceInterfaceDetail = ctypes.windll.setupapi.SetupDiGetDeviceInterfaceDetailA
SetupDiGetDeviceInterfaceDetail.argtypes = [HDEVINFO, PSP_DEVICE_INTERFACE_DATA, PSP_DEVICE_INTERFACE_DETAIL_DATA, DWORD, PDWORD, PSP_DEVINFO_DATA]
SetupDiGetDeviceInterfaceDetail.restype = BOOL
SetupDiGetDeviceRegistryProperty = ctypes.windll.setupapi.SetupDiGetDeviceRegistryPropertyA
SetupDiGetDeviceRegistryProperty.argtypes = [HDEVINFO, PSP_DEVINFO_DATA, DWORD, PDWORD, PBYTE, DWORD, PDWORD]
SetupDiGetDeviceRegistryProperty.restype = BOOL
GUID_CLASS_COMPORT = GUID(0x86e0d1e0L, 0x8089, 0x11d0,
(ctypes.c_ubyte*8)(0x9c, 0xe4, 0x08, 0x00, 0x3e, 0x30, 0x1f, 0x73))
DIGCF_PRESENT = 2
DIGCF_DEVICEINTERFACE = 16
INVALID_HANDLE_VALUE = 0
ERROR_INSUFFICIENT_BUFFER = 122
SPDRP_HARDWAREID = 1
SPDRP_FRIENDLYNAME = 12
SPDRP_LOCATION_INFORMATION = 13
ERROR_NO_MORE_ITEMS = 259
def comports(available_only=True):
"""This generator scans the device registry for com ports and yields
(order, port, desc, hwid). If available_only is true only return currently
existing ports. Order is a helper to get sorted lists. it can be ignored
otherwise."""
flags = DIGCF_DEVICEINTERFACE
if available_only:
flags |= DIGCF_PRESENT
g_hdi = SetupDiGetClassDevs(ctypes.byref(GUID_CLASS_COMPORT), None, NULL, flags);
#~ for i in range(256):
for dwIndex in range(256):
did = SP_DEVICE_INTERFACE_DATA()
did.cbSize = ctypes.sizeof(did)
if not SetupDiEnumDeviceInterfaces(
g_hdi,
None,
ctypes.byref(GUID_CLASS_COMPORT),
dwIndex,
ctypes.byref(did)
):
if ctypes.GetLastError() != ERROR_NO_MORE_ITEMS:
raise ctypes.WinError()
break
dwNeeded = DWORD()
# get the size
if not SetupDiGetDeviceInterfaceDetail(
g_hdi,
ctypes.byref(did),
None, 0, ctypes.byref(dwNeeded),
None
):
# Ignore ERROR_INSUFFICIENT_BUFFER
if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
raise ctypes.WinError()
# allocate buffer
class SP_DEVICE_INTERFACE_DETAIL_DATA_A(ctypes.Structure):
_fields_ = [
('cbSize', DWORD),
('DevicePath', CHAR*(dwNeeded.value - ctypes.sizeof(DWORD))),
]
def __str__(self):
return "DevicePath:%s" % (self.DevicePath,)
idd = SP_DEVICE_INTERFACE_DETAIL_DATA_A()
idd.cbSize = SIZEOF_SP_DEVICE_INTERFACE_DETAIL_DATA_A
devinfo = SP_DEVINFO_DATA()
devinfo.cbSize = ctypes.sizeof(devinfo)
if not SetupDiGetDeviceInterfaceDetail(
g_hdi,
ctypes.byref(did),
ctypes.byref(idd), dwNeeded, None,
ctypes.byref(devinfo)
):
raise ctypes.WinError()
# hardware ID
szHardwareID = ctypes.create_string_buffer(250)
if not SetupDiGetDeviceRegistryProperty(
g_hdi,
ctypes.byref(devinfo),
SPDRP_HARDWAREID,
None,
ctypes.byref(szHardwareID), ctypes.sizeof(szHardwareID) - 1,
None
):
# Ignore ERROR_INSUFFICIENT_BUFFER
if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
raise ctypes.WinError()
# friendly name
szFriendlyName = ctypes.create_string_buffer(1024)
if not SetupDiGetDeviceRegistryProperty(
g_hdi,
ctypes.byref(devinfo),
SPDRP_FRIENDLYNAME,
None,
ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1,
None
):
# Ignore ERROR_INSUFFICIENT_BUFFER
if ctypes.GetLastError() != ERROR_INSUFFICIENT_BUFFER:
#~ raise ctypes.WinError()
# not getting friendly name for com0com devices, try something else
szFriendlyName = ctypes.create_string_buffer(1024)
if SetupDiGetDeviceRegistryProperty(
g_hdi,
ctypes.byref(devinfo),
SPDRP_LOCATION_INFORMATION,
None,
ctypes.byref(szFriendlyName), ctypes.sizeof(szFriendlyName) - 1,
None
):
port_name = "\\\\.\\" + szFriendlyName.value
order = None
else:
port_name = szFriendlyName.value
order = None
else:
try:
m = re.search(r"\((.*?(\d+))\)", szFriendlyName.value)
#~ print szFriendlyName.value, m.groups()
port_name = m.group(1)
order = int(m.group(2))
except AttributeError, msg:
port_name = szFriendlyName.value
order = None
yield order, port_name, szFriendlyName.value, szHardwareID.value
SetupDiDestroyDeviceInfoList(g_hdi)
if __name__ == '__main__':
import serial
print "-"*78
print "Serial ports"
print "-"*78
for order, port, desc, hwid in sorted(comports()):
print "%-10s: %s (%s) ->" % (port, desc, hwid),
try:
serial.Serial(port) # test open
except serial.serialutil.SerialException:
print "can't be openend"
else:
print "Ready"
print
# list of all ports the system knows
print "-"*78
print "All serial ports (registry)"
print "-"*78
for order, port, desc, hwid in sorted(comports(False)):
print "%-10s: %s (%s)" % (port, desc, hwid)
mavlink/share/pyshared/pymavlink/tools/images/gtk-quit.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 1.0 KiB

mavlink/share/pyshared/pymavlink/tools/images/media-playback-pause.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 314 B

mavlink/share/pyshared/pymavlink/tools/images/media-playback-start.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 308 B

mavlink/share/pyshared/pymavlink/tools/images/media-playback-stop.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 305 B

mavlink/share/pyshared/pymavlink/tools/images/media-seek-backward.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 319 B

mavlink/share/pyshared/pymavlink/tools/images/media-seek-forward.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 322 B

mavlink/share/pyshared/pymavlink/tools/images/player_end.gif
BIN
View File
Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 555 B

mavlink/share/pyshared/pymavlink/tools/mavplayback.py
-246
View File
@@ -1,246 +0,0 @@
#!/usr/bin/env python
'''
play back a mavlink log as a FlightGear FG NET stream, and as a
realtime mavlink stream
Useful for visualising flights
'''
import sys, time, os, struct
import Tkinter
# 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 fgFDM
from optparse import OptionParser
parser = OptionParser("mavplayback.py [options]")
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 condition")
parser.add_option("--gpsalt", action='store_true', default=False, help="Use GPS altitude")
parser.add_option("--mav10", action='store_true', default=False, help="Use MAVLink protocol 1.0")
parser.add_option("--out", help="MAVLink output port (IP:port)",
action='append', default=['127.0.0.1:14550'])
parser.add_option("--fgout", action='append', default=['127.0.0.1:5503'],
help="flightgear FDM NET output (IP:port)")
parser.add_option("--baudrate", type='int', default=57600, help='baud rate')
(opts, args) = parser.parse_args()
if opts.mav10:
os.environ['MAVLINK10'] = '1'
import mavutil
if len(args) < 1:
parser.print_help()
sys.exit(1)
filename = args[0]
def LoadImage(filename):
'''return an image from the images/ directory'''
app_dir = os.path.dirname(os.path.realpath(__file__))
path = os.path.join(app_dir, 'files/images', filename)
return Tkinter.PhotoImage(file=path)
class App():
def __init__(self, filename):
self.root = Tkinter.Tk()
self.filesize = os.path.getsize(filename)
self.filepos = 0.0
self.mlog = mavutil.mavlink_connection(filename, planner_format=opts.planner,
robust_parsing=True)
self.mout = []
for m in opts.out:
self.mout.append(mavutil.mavlink_connection(m, input=False, baud=opts.baudrate))
self.fgout = []
for f in opts.fgout:
self.fgout.append(mavutil.mavudp(f, input=False))
self.fdm = fgFDM.fgFDM()
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is None:
sys.exit(1)
self.last_timestamp = getattr(self.msg, '_timestamp')
self.paused = False
self.topframe = Tkinter.Frame(self.root)
self.topframe.pack(side=Tkinter.TOP)
self.frame = Tkinter.Frame(self.root)
self.frame.pack(side=Tkinter.LEFT)
self.slider = Tkinter.Scale(self.topframe, from_=0, to=1.0, resolution=0.01,
orient=Tkinter.HORIZONTAL, command=self.slew)
self.slider.pack(side=Tkinter.LEFT)
self.clock = Tkinter.Label(self.topframe,text="")
self.clock.pack(side=Tkinter.RIGHT)
self.playback = Tkinter.Spinbox(self.topframe, from_=0, to=20, increment=0.1, width=3)
self.playback.pack(side=Tkinter.BOTTOM)
self.playback.delete(0, "end")
self.playback.insert(0, 1)
self.buttons = {}
self.button('quit', 'gtk-quit.gif', self.frame.quit)
self.button('pause', 'media-playback-pause.gif', self.pause)
self.button('rewind', 'media-seek-backward.gif', self.rewind)
self.button('forward', 'media-seek-forward.gif', self.forward)
self.button('status', 'Status', self.status)
self.flightmode = Tkinter.Label(self.frame,text="")
self.flightmode.pack(side=Tkinter.RIGHT)
self.next_message()
self.root.mainloop()
def button(self, name, filename, command):
'''add a button'''
try:
img = LoadImage(filename)
b = Tkinter.Button(self.frame, image=img, command=command)
b.image = img
except Exception:
b = Tkinter.Button(self.frame, text=filename, command=command)
b.pack(side=Tkinter.LEFT)
self.buttons[name] = b
def pause(self):
'''pause playback'''
self.paused = not self.paused
def rewind(self):
'''rewind 10%'''
pos = int(self.mlog.f.tell() - 0.1*self.filesize)
if pos < 0:
pos = 0
self.mlog.f.seek(pos)
self.find_message()
def forward(self):
'''forward 10%'''
pos = int(self.mlog.f.tell() + 0.1*self.filesize)
if pos > self.filesize:
pos = self.filesize - 2048
self.mlog.f.seek(pos)
self.find_message()
def status(self):
'''show status'''
for m in sorted(self.mlog.messages.keys()):
print(str(self.mlog.messages[m]))
def find_message(self):
'''find the next valid message'''
while True:
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is not None and self.msg.get_type() != 'BAD_DATA':
break
if self.mlog.f.tell() > self.filesize - 10:
self.paused = True
break
self.last_timestamp = getattr(self.msg, '_timestamp')
def slew(self, value):
'''move to a given position in the file'''
if float(value) != self.filepos:
pos = float(value) * self.filesize
self.mlog.f.seek(int(pos))
self.find_message()
def next_message(self):
'''called as each msg is ready'''
msg = self.msg
if msg is None:
self.paused = True
if self.paused:
self.root.after(100, self.next_message)
return
speed = float(self.playback.get())
timestamp = getattr(msg, '_timestamp')
now = time.strftime("%H:%M:%S", time.localtime(timestamp))
self.clock.configure(text=now)
if speed == 0.0:
self.root.after(200, self.next_message)
else:
self.root.after(int(1000*(timestamp - self.last_timestamp) / speed), self.next_message)
self.last_timestamp = timestamp
while True:
self.msg = self.mlog.recv_match(condition=opts.condition)
if self.msg is None and self.mlog.f.tell() > self.filesize - 10:
self.paused = True
return
if self.msg is not None and self.msg.get_type() != "BAD_DATA":
break
pos = float(self.mlog.f.tell()) / self.filesize
self.slider.set(pos)
self.filepos = self.slider.get()
if msg.get_type() != "BAD_DATA":
for m in self.mout:
m.write(msg.get_msgbuf().tostring())
if msg.get_type() == "GPS_RAW":
self.fdm.set('latitude', msg.lat, units='degrees')
self.fdm.set('longitude', msg.lon, units='degrees')
if opts.gpsalt:
self.fdm.set('altitude', msg.alt, units='meters')
if msg.get_type() == "VFR_HUD":
if not opts.gpsalt:
self.fdm.set('altitude', msg.alt, units='meters')
self.fdm.set('num_engines', 1)
self.fdm.set('vcas', msg.airspeed, units='mps')
if msg.get_type() == "ATTITUDE":
self.fdm.set('phi', msg.roll, units='radians')
self.fdm.set('theta', msg.pitch, units='radians')
self.fdm.set('psi', msg.yaw, units='radians')
self.fdm.set('phidot', msg.rollspeed, units='rps')
self.fdm.set('thetadot', msg.pitchspeed, units='rps')
self.fdm.set('psidot', msg.yawspeed, units='rps')
if msg.get_type() == "RC_CHANNELS_SCALED":
self.fdm.set("right_aileron", msg.chan1_scaled*0.0001)
self.fdm.set("left_aileron", -msg.chan1_scaled*0.0001)
self.fdm.set("rudder", msg.chan4_scaled*0.0001)
self.fdm.set("elevator", msg.chan2_scaled*0.0001)
self.fdm.set('rpm', msg.chan3_scaled*0.01)
if msg.get_type() == 'STATUSTEXT':
print("APM: %s" % msg.text)
if msg.get_type() == 'SYS_STATUS':
self.flightmode.configure(text=self.mlog.flightmode)
if msg.get_type() == "BAD_DATA":
if mavutil.all_printable(msg.data):
sys.stdout.write(msg.data)
sys.stdout.flush()
if self.fdm.get('latitude') != 0:
for f in self.fgout:
f.write(self.fdm.pack())
app=App(filename)