diff --git a/Tools/autotest/autotest.py b/Tools/autotest/autotest.py index 80cec3410d..618aca485d 100755 --- a/Tools/autotest/autotest.py +++ b/Tools/autotest/autotest.py @@ -6,7 +6,7 @@ import pexpect, os, sys, shutil, atexit import optparse, fnmatch, time, glob, traceback sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pysim')) -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), 'pymavlink')) +sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..', 'mavlink', 'pymavlink')) import util os.environ['PYTHONUNBUFFERED'] = '1' diff --git a/Tools/autotest/jsbsim/runsim.py b/Tools/autotest/jsbsim/runsim.py index 2ca40a1255..40ada9805c 100755 --- a/Tools/autotest/jsbsim/runsim.py +++ b/Tools/autotest/jsbsim/runsim.py @@ -5,7 +5,7 @@ import sys, os, pexpect, fdpexpect, socket import math, time, select, struct, signal, errno sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pysim')) -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pymavlink')) +sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..', '..', 'mavlink', 'pymavlink')) import util, fgFDM, atexit diff --git a/Tools/autotest/pymavlink/fgFDM.py b/Tools/autotest/pymavlink/fgFDM.py deleted file mode 100644 index ae1602e534..0000000000 --- a/Tools/autotest/pymavlink/fgFDM.py +++ /dev/null @@ -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) diff --git a/Tools/autotest/pymavlink/mavextra.py b/Tools/autotest/pymavlink/mavextra.py deleted file mode 100644 index b678f1a0fd..0000000000 --- a/Tools/autotest/pymavlink/mavextra.py +++ /dev/null @@ -1,199 +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(SCALED_PRESSURE): - '''calculate barometric altitude''' - import mavutil - self = mavutil.mavfile_global - if self.param('GND_ABS_PRESS', None) is None: - return 0 - scaling = self.param('GND_ABS_PRESS', 1) / (SCALED_PRESSURE.press_abs*100.0) - temp = self.param('GND_TEMP', 0) + 273.15 - return log(scaling) * temp * 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,SENSOR_OFFSETS=None, ofs=None, mul=None,smooth=0.7): - '''estimate roll from accelerometer''' - rx = RAW_IMU.xacc * 9.81 / 1000.0 - ry = RAW_IMU.yacc * 9.81 / 1000.0 - rz = RAW_IMU.zacc * 9.81 / 1000.0 - if SENSOR_OFFSETS is not None and ofs is not None: - rx += SENSOR_OFFSETS.accel_cal_x - ry += SENSOR_OFFSETS.accel_cal_y - rz += SENSOR_OFFSETS.accel_cal_z - rx -= ofs[0] - ry -= ofs[1] - rz -= ofs[2] - if mul is not None: - rx *= mul[0] - ry *= mul[1] - rz *= mul[2] - return lowpass(degrees(-asin(ry/sqrt(rx**2+ry**2+rz**2))),'_roll',smooth) - -def pitch_estimate(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7): - '''estimate pitch from accelerometer''' - rx = RAW_IMU.xacc * 9.81 / 1000.0 - ry = RAW_IMU.yacc * 9.81 / 1000.0 - rz = RAW_IMU.zacc * 9.81 / 1000.0 - if SENSOR_OFFSETS is not None and ofs is not None: - rx += SENSOR_OFFSETS.accel_cal_x - ry += SENSOR_OFFSETS.accel_cal_y - rz += SENSOR_OFFSETS.accel_cal_z - rx -= ofs[0] - ry -= ofs[1] - rz -= ofs[2] - if mul is not None: - rx *= mul[0] - ry *= mul[1] - rz *= mul[2] - return lowpass(degrees(asin(rx/sqrt(rx**2+ry**2+rz**2))),'_pitch',smooth) - -def gravity(RAW_IMU, SENSOR_OFFSETS=None, ofs=None, mul=None, smooth=0.7): - '''estimate pitch from accelerometer''' - rx = RAW_IMU.xacc * 9.81 / 1000.0 - ry = RAW_IMU.yacc * 9.81 / 1000.0 - rz = RAW_IMU.zacc * 9.81 / 1000.0 - if SENSOR_OFFSETS is not None and ofs is not None: - rx += SENSOR_OFFSETS.accel_cal_x - ry += SENSOR_OFFSETS.accel_cal_y - rz += SENSOR_OFFSETS.accel_cal_z - rx -= ofs[0] - ry -= ofs[1] - rz -= ofs[2] - if mul is not None: - rx *= mul[0] - ry *= mul[1] - rz *= mul[2] - return lowpass(sqrt(rx**2+ry**2+rz**2),'_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)**2 + sin(0.5*dLon)**2 * 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 GPS_RAW.fix_type < 2: - return 0 - if first_fix == None or first_fix.fix_type < 2: - first_fix = GPS_RAW - return 0 - return distance_two(GPS_RAW, first_fix) - -def sawtooth(ATTITUDE, amplitude=2.0, period=5.0): - '''sawtooth pattern based on uptime''' - mins = (ATTITUDE.usec * 1.0e-6)/60 - p = fmod(mins, period*2) - if p < period: - return amplitude * (p/period) - return amplitude * (period - (p-period))/period diff --git a/Tools/autotest/pymavlink/mavlink.py b/Tools/autotest/pymavlink/mavlink.py deleted file mode 100644 index f8bde1ce6a..0000000000 --- a/Tools/autotest/pymavlink/mavlink.py +++ /dev/null @@ -1,4947 +0,0 @@ -''' -MAVLink protocol implementation (auto-generated by mavgen.py) - -Generated from: ardupilotmega.xml,common.xml - -Note: this file has been auto-generated. DO NOT EDIT -''' - -import struct, array, mavutil, time - -WIRE_PROTOCOL_VERSION = "0.9" - - -# some base types from mavlink_types.h -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 - - -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', 85, 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): - if isinstance(self._msgbuf, str): - return self._msgbuf - return self._msgbuf.tostring() - - 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 False: # using CRC extra - crc.accumulate(chr(crc_extra)) - self._crc = crc.crc - self._msgbuf += struct.pack('hhhfiiffffff', self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z, self.mag_declination, self.raw_press, self.raw_temp, self.gyro_cal_x, self.gyro_cal_y, self.gyro_cal_z, self.accel_cal_x, self.accel_cal_y, self.accel_cal_z)) - -class MAVLink_set_mag_offsets_message(MAVLink_message): - ''' - set the magnetometer offsets - ''' - def __init__(self, target_system, target_component, mag_ofs_x, mag_ofs_y, mag_ofs_z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MAG_OFFSETS, 'SET_MAG_OFFSETS') - self._fieldnames = ['target_system', 'target_component', 'mag_ofs_x', 'mag_ofs_y', 'mag_ofs_z'] - self.target_system = target_system - self.target_component = target_component - self.mag_ofs_x = mag_ofs_x - self.mag_ofs_y = mag_ofs_y - self.mag_ofs_z = mag_ofs_z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 29, struct.pack('>BBhhh', self.target_system, self.target_component, self.mag_ofs_x, self.mag_ofs_y, self.mag_ofs_z)) - -class MAVLink_meminfo_message(MAVLink_message): - ''' - state of APM memory - ''' - def __init__(self, brkval, freemem): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MEMINFO, 'MEMINFO') - self._fieldnames = ['brkval', 'freemem'] - self.brkval = brkval - self.freemem = freemem - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 208, struct.pack('>HH', self.brkval, self.freemem)) - -class MAVLink_ap_adc_message(MAVLink_message): - ''' - raw ADC output - ''' - def __init__(self, adc1, adc2, adc3, adc4, adc5, adc6): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AP_ADC, 'AP_ADC') - self._fieldnames = ['adc1', 'adc2', 'adc3', 'adc4', 'adc5', 'adc6'] - self.adc1 = adc1 - self.adc2 = adc2 - self.adc3 = adc3 - self.adc4 = adc4 - self.adc5 = adc5 - self.adc6 = adc6 - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 188, struct.pack('>HHHHHH', self.adc1, self.adc2, self.adc3, self.adc4, self.adc5, self.adc6)) - -class MAVLink_digicam_configure_message(MAVLink_message): - ''' - Configure on-board Camera Control System. - ''' - def __init__(self, target_system, target_component, mode, shutter_speed, aperture, iso, exposure_type, command_id, engine_cut_off, extra_param, extra_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONFIGURE, 'DIGICAM_CONFIGURE') - self._fieldnames = ['target_system', 'target_component', 'mode', 'shutter_speed', 'aperture', 'iso', 'exposure_type', 'command_id', 'engine_cut_off', 'extra_param', 'extra_value'] - self.target_system = target_system - self.target_component = target_component - self.mode = mode - self.shutter_speed = shutter_speed - self.aperture = aperture - self.iso = iso - self.exposure_type = exposure_type - self.command_id = command_id - self.engine_cut_off = engine_cut_off - self.extra_param = extra_param - self.extra_value = extra_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 118, struct.pack('>BBBHBBBBBBf', self.target_system, self.target_component, self.mode, self.shutter_speed, self.aperture, self.iso, self.exposure_type, self.command_id, self.engine_cut_off, self.extra_param, self.extra_value)) - -class MAVLink_digicam_control_message(MAVLink_message): - ''' - Control on-board Camera Control System to take shots. - ''' - def __init__(self, target_system, target_component, session, zoom_pos, zoom_step, focus_lock, shot, command_id, extra_param, extra_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DIGICAM_CONTROL, 'DIGICAM_CONTROL') - self._fieldnames = ['target_system', 'target_component', 'session', 'zoom_pos', 'zoom_step', 'focus_lock', 'shot', 'command_id', 'extra_param', 'extra_value'] - self.target_system = target_system - self.target_component = target_component - self.session = session - self.zoom_pos = zoom_pos - self.zoom_step = zoom_step - self.focus_lock = focus_lock - self.shot = shot - self.command_id = command_id - self.extra_param = extra_param - self.extra_value = extra_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 242, struct.pack('>BBBBbBBBBf', self.target_system, self.target_component, self.session, self.zoom_pos, self.zoom_step, self.focus_lock, self.shot, self.command_id, self.extra_param, self.extra_value)) - -class MAVLink_mount_configure_message(MAVLink_message): - ''' - Message to configure a camera mount, directional antenna, etc. - ''' - def __init__(self, target_system, target_component, mount_mode, stab_roll, stab_pitch, stab_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONFIGURE, 'MOUNT_CONFIGURE') - self._fieldnames = ['target_system', 'target_component', 'mount_mode', 'stab_roll', 'stab_pitch', 'stab_yaw'] - self.target_system = target_system - self.target_component = target_component - self.mount_mode = mount_mode - self.stab_roll = stab_roll - self.stab_pitch = stab_pitch - self.stab_yaw = stab_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 19, struct.pack('>BBBBBB', self.target_system, self.target_component, self.mount_mode, self.stab_roll, self.stab_pitch, self.stab_yaw)) - -class MAVLink_mount_control_message(MAVLink_message): - ''' - Message to control a camera mount, directional antenna, etc. - ''' - def __init__(self, target_system, target_component, input_a, input_b, input_c, save_position): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_CONTROL, 'MOUNT_CONTROL') - self._fieldnames = ['target_system', 'target_component', 'input_a', 'input_b', 'input_c', 'save_position'] - self.target_system = target_system - self.target_component = target_component - self.input_a = input_a - self.input_b = input_b - self.input_c = input_c - self.save_position = save_position - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 97, struct.pack('>BBiiiB', self.target_system, self.target_component, self.input_a, self.input_b, self.input_c, self.save_position)) - -class MAVLink_mount_status_message(MAVLink_message): - ''' - Message with some status from APM to GCS about camera or - antenna mount - ''' - def __init__(self, target_system, target_component, pointing_a, pointing_b, pointing_c): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MOUNT_STATUS, 'MOUNT_STATUS') - self._fieldnames = ['target_system', 'target_component', 'pointing_a', 'pointing_b', 'pointing_c'] - self.target_system = target_system - self.target_component = target_component - self.pointing_a = pointing_a - self.pointing_b = pointing_b - self.pointing_c = pointing_c - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 233, struct.pack('>BBiii', self.target_system, self.target_component, self.pointing_a, self.pointing_b, self.pointing_c)) - -class MAVLink_fence_point_message(MAVLink_message): - ''' - A fence point. Used to set a point when from GCS - -> MAV. Also used to return a point from MAV -> GCS - ''' - def __init__(self, target_system, target_component, idx, count, lat, lng): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - self.count = count - self.lat = lat - self.lng = lng - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 18, struct.pack('>BBBBff', self.target_system, self.target_component, self.idx, self.count, self.lat, self.lng)) - -class MAVLink_fence_fetch_point_message(MAVLink_message): - ''' - Request a current fence point from MAV - ''' - def __init__(self, target_system, target_component, idx): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_FETCH_POINT, 'FENCE_FETCH_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 68, struct.pack('>BBB', self.target_system, self.target_component, self.idx)) - -class MAVLink_fence_status_message(MAVLink_message): - ''' - Status of geo-fencing. Sent in extended status - stream when fencing enabled - ''' - def __init__(self, breach_status, breach_count, breach_type, breach_time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_STATUS, 'FENCE_STATUS') - self._fieldnames = ['breach_status', 'breach_count', 'breach_type', 'breach_time'] - self.breach_status = breach_status - self.breach_count = breach_count - self.breach_type = breach_type - self.breach_time = breach_time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 136, struct.pack('>BHBI', self.breach_status, self.breach_count, self.breach_type, self.breach_time)) - -class MAVLink_ahrs_message(MAVLink_message): - ''' - Status of DCM attitude estimator - ''' - def __init__(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AHRS, 'AHRS') - self._fieldnames = ['omegaIx', 'omegaIy', 'omegaIz', 'accel_weight', 'renorm_val', 'error_rp', 'error_yaw'] - self.omegaIx = omegaIx - self.omegaIy = omegaIy - self.omegaIz = omegaIz - self.accel_weight = accel_weight - self.renorm_val = renorm_val - self.error_rp = error_rp - self.error_yaw = error_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 127, struct.pack('>fffffff', self.omegaIx, self.omegaIy, self.omegaIz, self.accel_weight, self.renorm_val, self.error_rp, self.error_yaw)) - -class MAVLink_simstate_message(MAVLink_message): - ''' - Status of simulation environment, if used - ''' - def __init__(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SIMSTATE, 'SIMSTATE') - self._fieldnames = ['roll', 'pitch', 'yaw', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro'] - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 42, struct.pack('>fffffffff', self.roll, self.pitch, self.yaw, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro)) - -class MAVLink_hwstatus_message(MAVLink_message): - ''' - Status of key hardware - ''' - def __init__(self, Vcc, I2Cerr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HWSTATUS, 'HWSTATUS') - self._fieldnames = ['Vcc', 'I2Cerr'] - self.Vcc = Vcc - self.I2Cerr = I2Cerr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 21, struct.pack('>HB', self.Vcc, self.I2Cerr)) - -class MAVLink_radio_message(MAVLink_message): - ''' - Status generated by radio - ''' - def __init__(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RADIO, 'RADIO') - self._fieldnames = ['rssi', 'remrssi', 'txbuf', 'noise', 'remnoise', 'rxerrors', 'fixed'] - self.rssi = rssi - self.remrssi = remrssi - self.txbuf = txbuf - self.noise = noise - self.remnoise = remnoise - self.rxerrors = rxerrors - self.fixed = fixed - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 93, struct.pack('>BBBBBHH', self.rssi, self.remrssi, self.txbuf, self.noise, self.remnoise, self.rxerrors, self.fixed)) - -class MAVLink_heartbeat_message(MAVLink_message): - ''' - The heartbeat message shows that a system is present and - responding. The type of the MAV and Autopilot hardware allow - the receiving system to treat further messages from this - system appropriate (e.g. by laying out the user interface - based on the autopilot). - ''' - def __init__(self, type, autopilot, mavlink_version): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HEARTBEAT, 'HEARTBEAT') - self._fieldnames = ['type', 'autopilot', 'mavlink_version'] - self.type = type - self.autopilot = autopilot - self.mavlink_version = mavlink_version - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 72, struct.pack('>BBB', self.type, self.autopilot, self.mavlink_version)) - -class MAVLink_boot_message(MAVLink_message): - ''' - The boot message indicates that a system is starting. The - onboard software version allows to keep track of onboard - soft/firmware revisions. - ''' - def __init__(self, version): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_BOOT, 'BOOT') - self._fieldnames = ['version'] - self.version = version - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 39, struct.pack('>I', self.version)) - -class MAVLink_system_time_message(MAVLink_message): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - ''' - def __init__(self, time_usec): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME, 'SYSTEM_TIME') - self._fieldnames = ['time_usec'] - self.time_usec = time_usec - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 190, struct.pack('>Q', self.time_usec)) - -class MAVLink_ping_message(MAVLink_message): - ''' - A ping message either requesting or responding to a ping. This - allows to measure the system latencies, including serial port, - radio modem and UDP connections. - ''' - def __init__(self, seq, target_system, target_component, time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PING, 'PING') - self._fieldnames = ['seq', 'target_system', 'target_component', 'time'] - self.seq = seq - self.target_system = target_system - self.target_component = target_component - self.time = time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 92, struct.pack('>IBBQ', self.seq, self.target_system, self.target_component, self.time)) - -class MAVLink_system_time_utc_message(MAVLink_message): - ''' - UTC date and time from GPS module - ''' - def __init__(self, utc_date, utc_time): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYSTEM_TIME_UTC, 'SYSTEM_TIME_UTC') - self._fieldnames = ['utc_date', 'utc_time'] - self.utc_date = utc_date - self.utc_time = utc_time - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 191, struct.pack('>II', self.utc_date, self.utc_time)) - -class MAVLink_change_operator_control_message(MAVLink_message): - ''' - Request to control this MAV - ''' - def __init__(self, target_system, control_request, version, passkey): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL, 'CHANGE_OPERATOR_CONTROL') - self._fieldnames = ['target_system', 'control_request', 'version', 'passkey'] - self.target_system = target_system - self.control_request = control_request - self.version = version - self.passkey = passkey - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 217, struct.pack('>BBB25s', self.target_system, self.control_request, self.version, self.passkey)) - -class MAVLink_change_operator_control_ack_message(MAVLink_message): - ''' - Accept / deny control of this MAV - ''' - def __init__(self, gcs_system_id, control_request, ack): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK, 'CHANGE_OPERATOR_CONTROL_ACK') - self._fieldnames = ['gcs_system_id', 'control_request', 'ack'] - self.gcs_system_id = gcs_system_id - self.control_request = control_request - self.ack = ack - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 104, struct.pack('>BBB', self.gcs_system_id, self.control_request, self.ack)) - -class MAVLink_auth_key_message(MAVLink_message): - ''' - Emit an encrypted signature / key identifying this system. - PLEASE NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for true - safety. - ''' - def __init__(self, key): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_AUTH_KEY, 'AUTH_KEY') - self._fieldnames = ['key'] - self.key = key - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 119, struct.pack('>32s', self.key)) - -class MAVLink_action_ack_message(MAVLink_message): - ''' - This message acknowledges an action. IMPORTANT: The - acknowledgement can be also negative, e.g. the MAV rejects a - reset message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - ''' - def __init__(self, action, result): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION_ACK, 'ACTION_ACK') - self._fieldnames = ['action', 'result'] - self.action = action - self.result = result - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 219, struct.pack('>BB', self.action, self.result)) - -class MAVLink_action_message(MAVLink_message): - ''' - An action message allows to execute a certain onboard action. - These include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM MAV_ACTION - in mavlink/include/mavlink_types.h - ''' - def __init__(self, target, target_component, action): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ACTION, 'ACTION') - self._fieldnames = ['target', 'target_component', 'action'] - self.target = target - self.target_component = target_component - self.action = action - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 60, struct.pack('>BBB', self.target, self.target_component, self.action)) - -class MAVLink_set_mode_message(MAVLink_message): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target component - id as the mode is by definition for the overall aircraft, not - only for one component. - ''' - def __init__(self, target, mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_MODE, 'SET_MODE') - self._fieldnames = ['target', 'mode'] - self.target = target - self.mode = mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 186, struct.pack('>BB', self.target, self.mode)) - -class MAVLink_set_nav_mode_message(MAVLink_message): - ''' - Set the system navigation mode, as defined by enum - MAV_NAV_MODE in mavlink/include/mavlink_types.h. The - navigation mode applies to the whole aircraft and thus all - components. - ''' - def __init__(self, target, nav_mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_NAV_MODE, 'SET_NAV_MODE') - self._fieldnames = ['target', 'nav_mode'] - self.target = target - self.nav_mode = nav_mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 10, struct.pack('>BB', self.target, self.nav_mode)) - -class MAVLink_param_request_read_message(MAVLink_message): - ''' - Request to read the onboard parameter with the param_id string - id. Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any other - component (such as the GCS) without the need of previous - knowledge of possible parameter names. Thus the same GCS can - store different parameters for different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a full - documentation of QGroundControl and IMU code. - ''' - def __init__(self, target_system, target_component, param_id, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 89, struct.pack('>BB15sh', self.target_system, self.target_component, self.param_id, self.param_index)) - -class MAVLink_param_request_list_message(MAVLink_message): - ''' - Request all parameters of this component. After his request, - all parameters are emitted. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_LIST, 'PARAM_REQUEST_LIST') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 159, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_param_value_message(MAVLink_message): - ''' - Emit the value of a onboard parameter. The inclusion of - param_count and param_index in the message allows the - recipient to keep track of received parameters and allows him - to re-request missing parameters after a loss or timeout. - ''' - def __init__(self, param_id, param_value, param_count, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_VALUE, 'PARAM_VALUE') - self._fieldnames = ['param_id', 'param_value', 'param_count', 'param_index'] - self.param_id = param_id - self.param_value = param_value - self.param_count = param_count - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 162, struct.pack('>15sfHH', self.param_id, self.param_value, self.param_count, self.param_index)) - -class MAVLink_param_set_message(MAVLink_message): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to - default on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents - to EEPROM. IMPORTANT: The receiving component should - acknowledge the new parameter value by sending a param_value - message to all communication partners. This will also ensure - that multiple GCS all have an up-to-date list of all - parameters. If the sending GCS did not receive a PARAM_VALUE - message within its timeout time, it should re-send the - PARAM_SET message. - ''' - def __init__(self, target_system, target_component, param_id, param_value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_SET, 'PARAM_SET') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_value'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_value = param_value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 121, struct.pack('>BB15sf', self.target_system, self.target_component, self.param_id, self.param_value)) - -class MAVLink_gps_raw_int_message(MAVLink_message): - ''' - The global position, as returned by the Global Positioning - System (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. Coordinate - frame is right-handed, Z-axis up (GPS frame) - ''' - def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW_INT, 'GPS_RAW_INT') - self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg'] - self.usec = usec - self.fix_type = fix_type - self.lat = lat - self.lon = lon - self.alt = alt - self.eph = eph - self.epv = epv - self.v = v - self.hdg = hdg - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 149, struct.pack('>QBiiiffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg)) - -class MAVLink_scaled_imu_message(MAVLink_message): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This - message should contain the scaled values to the described - units - ''' - def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_IMU, 'SCALED_IMU') - self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag'] - self.usec = usec - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - self.xmag = xmag - self.ymag = ymag - self.zmag = zmag - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 222, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag)) - -class MAVLink_gps_status_message(MAVLink_message): - ''' - The positioning status, as reported by GPS. This message is - intended to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION for the - global position estimate. This message can contain information - for up to 20 satellites. - ''' - def __init__(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_STATUS, 'GPS_STATUS') - self._fieldnames = ['satellites_visible', 'satellite_prn', 'satellite_used', 'satellite_elevation', 'satellite_azimuth', 'satellite_snr'] - self.satellites_visible = satellites_visible - self.satellite_prn = satellite_prn - self.satellite_used = satellite_used - self.satellite_elevation = satellite_elevation - self.satellite_azimuth = satellite_azimuth - self.satellite_snr = satellite_snr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 110, struct.pack('>B20s20s20s20s20s', self.satellites_visible, self.satellite_prn, self.satellite_used, self.satellite_elevation, self.satellite_azimuth, self.satellite_snr)) - -class MAVLink_raw_imu_message(MAVLink_message): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This - message should always contain the true raw values without any - scaling to allow data capture and system debugging. - ''' - def __init__(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_IMU, 'RAW_IMU') - self._fieldnames = ['usec', 'xacc', 'yacc', 'zacc', 'xgyro', 'ygyro', 'zgyro', 'xmag', 'ymag', 'zmag'] - self.usec = usec - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - self.xgyro = xgyro - self.ygyro = ygyro - self.zgyro = zgyro - self.xmag = xmag - self.ymag = ymag - self.zmag = zmag - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 179, struct.pack('>Qhhhhhhhhh', self.usec, self.xacc, self.yacc, self.zacc, self.xgyro, self.ygyro, self.zgyro, self.xmag, self.ymag, self.zmag)) - -class MAVLink_raw_pressure_message(MAVLink_message): - ''' - The RAW pressure readings for the typical setup of one - absolute pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - ''' - def __init__(self, usec, press_abs, press_diff1, press_diff2, temperature): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RAW_PRESSURE, 'RAW_PRESSURE') - self._fieldnames = ['usec', 'press_abs', 'press_diff1', 'press_diff2', 'temperature'] - self.usec = usec - self.press_abs = press_abs - self.press_diff1 = press_diff1 - self.press_diff2 = press_diff2 - self.temperature = temperature - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 136, struct.pack('>Qhhhh', self.usec, self.press_abs, self.press_diff1, self.press_diff2, self.temperature)) - -class MAVLink_scaled_pressure_message(MAVLink_message): - ''' - The pressure readings for the typical setup of one absolute - and differential pressure sensor. The units are as specified - in each field. - ''' - def __init__(self, usec, press_abs, press_diff, temperature): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SCALED_PRESSURE, 'SCALED_PRESSURE') - self._fieldnames = ['usec', 'press_abs', 'press_diff', 'temperature'] - self.usec = usec - self.press_abs = press_abs - self.press_diff = press_diff - self.temperature = temperature - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 229, struct.pack('>Qffh', self.usec, self.press_abs, self.press_diff, self.temperature)) - -class MAVLink_attitude_message(MAVLink_message): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, - X-front, Y-right). - ''' - def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ATTITUDE, 'ATTITUDE') - self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed'] - self.usec = usec - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.rollspeed = rollspeed - self.pitchspeed = pitchspeed - self.yawspeed = yawspeed - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 66, struct.pack('>Qffffff', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed)) - -class MAVLink_local_position_message(MAVLink_message): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, Z-axis down - (aeronautical frame) - ''' - def __init__(self, usec, x, y, z, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION, 'LOCAL_POSITION') - self._fieldnames = ['usec', 'x', 'y', 'z', 'vx', 'vy', 'vz'] - self.usec = usec - self.x = x - self.y = y - self.z = z - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>Qffffff', self.usec, self.x, self.y, self.z, self.vx, self.vy, self.vz)) - -class MAVLink_global_position_message(MAVLink_message): - ''' - The filtered global position (e.g. fused GPS and - accelerometers). Coordinate frame is right-handed, Z-axis up - (GPS frame) - ''' - def __init__(self, usec, lat, lon, alt, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION, 'GLOBAL_POSITION') - self._fieldnames = ['usec', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz'] - self.usec = usec - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 147, struct.pack('>Qffffff', self.usec, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz)) - -class MAVLink_gps_raw_message(MAVLink_message): - ''' - The global position, as returned by the Global Positioning - System (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. Coordinate - frame is right-handed, Z-axis up (GPS frame) - ''' - def __init__(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_RAW, 'GPS_RAW') - self._fieldnames = ['usec', 'fix_type', 'lat', 'lon', 'alt', 'eph', 'epv', 'v', 'hdg'] - self.usec = usec - self.fix_type = fix_type - self.lat = lat - self.lon = lon - self.alt = alt - self.eph = eph - self.epv = epv - self.v = v - self.hdg = hdg - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 185, struct.pack('>QBfffffff', self.usec, self.fix_type, self.lat, self.lon, self.alt, self.eph, self.epv, self.v, self.hdg)) - -class MAVLink_sys_status_message(MAVLink_message): - ''' - The general system state. If the system is following the - MAVLink standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is either - LOCKED (motors shut down and locked), MANUAL (system under RC - control), GUIDED (system with autonomous position control, - position setpoint controlled manually) or AUTO (system guided - by path/waypoint planner). The NAV_MODE defined the current - flight state: LIFTOFF (often an open-loop maneuver), LANDING, - WAYPOINTS or VECTOR. This represents the internal navigation - state machine. The system status shows wether the system is - currently active or not and if an emergency occured. During - the CRITICAL and EMERGENCY states the MAV is still considered - to be active, but should start emergency procedures - autonomously. After a failure occured it should first move - from active to critical to allow manual intervention and then - move to emergency after a certain timeout. - ''' - def __init__(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SYS_STATUS, 'SYS_STATUS') - self._fieldnames = ['mode', 'nav_mode', 'status', 'load', 'vbat', 'battery_remaining', 'packet_drop'] - self.mode = mode - self.nav_mode = nav_mode - self.status = status - self.load = load - self.vbat = vbat - self.battery_remaining = battery_remaining - self.packet_drop = packet_drop - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 112, struct.pack('>BBBHHHH', self.mode, self.nav_mode, self.status, self.load, self.vbat, self.battery_remaining, self.packet_drop)) - -class MAVLink_rc_channels_raw_message(MAVLink_message): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters might - violate this specification. - ''' - def __init__(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_RAW, 'RC_CHANNELS_RAW') - self._fieldnames = ['chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw', 'rssi'] - self.chan1_raw = chan1_raw - self.chan2_raw = chan2_raw - self.chan3_raw = chan3_raw - self.chan4_raw = chan4_raw - self.chan5_raw = chan5_raw - self.chan6_raw = chan6_raw - self.chan7_raw = chan7_raw - self.chan8_raw = chan8_raw - self.rssi = rssi - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 252, struct.pack('>HHHHHHHHB', self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw, self.rssi)) - -class MAVLink_rc_channels_scaled_message(MAVLink_message): - ''' - The scaled values of the RC channels received. (-100%) -10000, - (0%) 0, (100%) 10000 - ''' - def __init__(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_SCALED, 'RC_CHANNELS_SCALED') - self._fieldnames = ['chan1_scaled', 'chan2_scaled', 'chan3_scaled', 'chan4_scaled', 'chan5_scaled', 'chan6_scaled', 'chan7_scaled', 'chan8_scaled', 'rssi'] - self.chan1_scaled = chan1_scaled - self.chan2_scaled = chan2_scaled - self.chan3_scaled = chan3_scaled - self.chan4_scaled = chan4_scaled - self.chan5_scaled = chan5_scaled - self.chan6_scaled = chan6_scaled - self.chan7_scaled = chan7_scaled - self.chan8_scaled = chan8_scaled - self.rssi = rssi - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 162, struct.pack('>hhhhhhhhB', self.chan1_scaled, self.chan2_scaled, self.chan3_scaled, self.chan4_scaled, self.chan5_scaled, self.chan6_scaled, self.chan7_scaled, self.chan8_scaled, self.rssi)) - -class MAVLink_servo_output_raw_message(MAVLink_message): - ''' - The RAW values of the servo outputs (for RC input from the - remote, use the RC_CHANNELS messages). The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - ''' - def __init__(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 'SERVO_OUTPUT_RAW') - self._fieldnames = ['servo1_raw', 'servo2_raw', 'servo3_raw', 'servo4_raw', 'servo5_raw', 'servo6_raw', 'servo7_raw', 'servo8_raw'] - self.servo1_raw = servo1_raw - self.servo2_raw = servo2_raw - self.servo3_raw = servo3_raw - self.servo4_raw = servo4_raw - self.servo5_raw = servo5_raw - self.servo6_raw = servo6_raw - self.servo7_raw = servo7_raw - self.servo8_raw = servo8_raw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 215, struct.pack('>HHHHHHHH', self.servo1_raw, self.servo2_raw, self.servo3_raw, self.servo4_raw, self.servo5_raw, self.servo6_raw, self.servo7_raw, self.servo8_raw)) - -class MAVLink_waypoint_message(MAVLink_message): - ''' - Message encoding a waypoint. This message is emitted to - announce the presence of a waypoint and to set a waypoint - on the system. The waypoint can be either in x, y, z meters - (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is - Z-down, right handed, global frame is Z-up, right handed - ''' - def __init__(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT, 'WAYPOINT') - self._fieldnames = ['target_system', 'target_component', 'seq', 'frame', 'command', 'current', 'autocontinue', 'param1', 'param2', 'param3', 'param4', 'x', 'y', 'z'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - self.frame = frame - self.command = command - self.current = current - self.autocontinue = autocontinue - self.param1 = param1 - self.param2 = param2 - self.param3 = param3 - self.param4 = param4 - self.x = x - self.y = y - self.z = z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 128, struct.pack('>BBHBBBBfffffff', self.target_system, self.target_component, self.seq, self.frame, self.command, self.current, self.autocontinue, self.param1, self.param2, self.param3, self.param4, self.x, self.y, self.z)) - -class MAVLink_waypoint_request_message(MAVLink_message): - ''' - Request the information of the waypoint with the sequence - number seq. The response of the system to this message should - be a WAYPOINT message. - ''' - def __init__(self, target_system, target_component, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST, 'WAYPOINT_REQUEST') - self._fieldnames = ['target_system', 'target_component', 'seq'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 9, struct.pack('>BBH', self.target_system, self.target_component, self.seq)) - -class MAVLink_waypoint_set_current_message(MAVLink_message): - ''' - Set the waypoint with sequence number seq as current waypoint. - This means that the MAV will continue to this waypoint on the - shortest path (not following the waypoints in-between). - ''' - def __init__(self, target_system, target_component, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT, 'WAYPOINT_SET_CURRENT') - self._fieldnames = ['target_system', 'target_component', 'seq'] - self.target_system = target_system - self.target_component = target_component - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 106, struct.pack('>BBH', self.target_system, self.target_component, self.seq)) - -class MAVLink_waypoint_current_message(MAVLink_message): - ''' - Message that announces the sequence number of the current - active waypoint. The MAV will fly towards this waypoint. - ''' - def __init__(self, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CURRENT, 'WAYPOINT_CURRENT') - self._fieldnames = ['seq'] - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 101, struct.pack('>H', self.seq)) - -class MAVLink_waypoint_request_list_message(MAVLink_message): - ''' - Request the overall list of waypoints from the - system/component. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST, 'WAYPOINT_REQUEST_LIST') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 213, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_waypoint_count_message(MAVLink_message): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST - by the MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of waypoints. - ''' - def __init__(self, target_system, target_component, count): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_COUNT, 'WAYPOINT_COUNT') - self._fieldnames = ['target_system', 'target_component', 'count'] - self.target_system = target_system - self.target_component = target_component - self.count = count - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 4, struct.pack('>BBH', self.target_system, self.target_component, self.count)) - -class MAVLink_waypoint_clear_all_message(MAVLink_message): - ''' - Delete all waypoints at once. - ''' - def __init__(self, target_system, target_component): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL, 'WAYPOINT_CLEAR_ALL') - self._fieldnames = ['target_system', 'target_component'] - self.target_system = target_system - self.target_component = target_component - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 229, struct.pack('>BB', self.target_system, self.target_component)) - -class MAVLink_waypoint_reached_message(MAVLink_message): - ''' - A certain waypoint has been reached. The system will either - hold this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next waypoint. - ''' - def __init__(self, seq): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_REACHED, 'WAYPOINT_REACHED') - self._fieldnames = ['seq'] - self.seq = seq - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 21, struct.pack('>H', self.seq)) - -class MAVLink_waypoint_ack_message(MAVLink_message): - ''' - Ack message during waypoint handling. The type field states if - this message is a positive ack (type=0) or if an error - happened (type=non-zero). - ''' - def __init__(self, target_system, target_component, type): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_WAYPOINT_ACK, 'WAYPOINT_ACK') - self._fieldnames = ['target_system', 'target_component', 'type'] - self.target_system = target_system - self.target_component = target_component - self.type = type - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 214, struct.pack('>BBB', self.target_system, self.target_component, self.type)) - -class MAVLink_gps_set_global_origin_message(MAVLink_message): - ''' - As local waypoints exist, the global waypoint reference allows - to transform between the local coordinate frame and the global - (GPS) coordinate frame. This can be necessary when e.g. in- - and outdoor settings are connected and the MAV should move - from in- to outdoor. - ''' - def __init__(self, target_system, target_component, latitude, longitude, altitude): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN, 'GPS_SET_GLOBAL_ORIGIN') - self._fieldnames = ['target_system', 'target_component', 'latitude', 'longitude', 'altitude'] - self.target_system = target_system - self.target_component = target_component - self.latitude = latitude - self.longitude = longitude - self.altitude = altitude - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 215, struct.pack('>BBiii', self.target_system, self.target_component, self.latitude, self.longitude, self.altitude)) - -class MAVLink_gps_local_origin_set_message(MAVLink_message): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - ''' - def __init__(self, latitude, longitude, altitude): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET, 'GPS_LOCAL_ORIGIN_SET') - self._fieldnames = ['latitude', 'longitude', 'altitude'] - self.latitude = latitude - self.longitude = longitude - self.altitude = altitude - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 14, struct.pack('>iii', self.latitude, self.longitude, self.altitude)) - -class MAVLink_local_position_setpoint_set_message(MAVLink_message): - ''' - Set the setpoint for a local position controller. This is the - position in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the onboard - position controller. As some MAVs have a degree of freedom in - yaw (e.g. all helicopters/quadrotors), the desired yaw angle - is part of the message. - ''' - def __init__(self, target_system, target_component, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET, 'LOCAL_POSITION_SETPOINT_SET') - self._fieldnames = ['target_system', 'target_component', 'x', 'y', 'z', 'yaw'] - self.target_system = target_system - self.target_component = target_component - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 206, struct.pack('>BBffff', self.target_system, self.target_component, self.x, self.y, self.z, self.yaw)) - -class MAVLink_local_position_setpoint_message(MAVLink_message): - ''' - Transmit the current local setpoint of the controller to other - MAVs (collision avoidance) and to the GCS. - ''' - def __init__(self, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, 'LOCAL_POSITION_SETPOINT') - self._fieldnames = ['x', 'y', 'z', 'yaw'] - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 50, struct.pack('>ffff', self.x, self.y, self.z, self.yaw)) - -class MAVLink_control_status_message(MAVLink_message): - ''' - - ''' - def __init__(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_CONTROL_STATUS, 'CONTROL_STATUS') - self._fieldnames = ['position_fix', 'vision_fix', 'gps_fix', 'ahrs_health', 'control_att', 'control_pos_xy', 'control_pos_z', 'control_pos_yaw'] - self.position_fix = position_fix - self.vision_fix = vision_fix - self.gps_fix = gps_fix - self.ahrs_health = ahrs_health - self.control_att = control_att - self.control_pos_xy = control_pos_xy - self.control_pos_z = control_pos_z - self.control_pos_yaw = control_pos_yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 157, struct.pack('>BBBBBBBB', self.position_fix, self.vision_fix, self.gps_fix, self.ahrs_health, self.control_att, self.control_pos_xy, self.control_pos_z, self.control_pos_yaw)) - -class MAVLink_safety_set_allowed_area_message(MAVLink_message): - ''' - Set a safety zone (volume), which is defined by two corners of - a cube. This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. Safety - areas are often enforced by national or competition - regulations. - ''' - def __init__(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA, 'SAFETY_SET_ALLOWED_AREA') - self._fieldnames = ['target_system', 'target_component', 'frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z'] - self.target_system = target_system - self.target_component = target_component - self.frame = frame - self.p1x = p1x - self.p1y = p1y - self.p1z = p1z - self.p2x = p2x - self.p2y = p2y - self.p2z = p2z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>BBBffffff', self.target_system, self.target_component, self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z)) - -class MAVLink_safety_allowed_area_message(MAVLink_message): - ''' - Read out the safety zone the MAV currently assumes. - ''' - def __init__(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA, 'SAFETY_ALLOWED_AREA') - self._fieldnames = ['frame', 'p1x', 'p1y', 'p1z', 'p2x', 'p2y', 'p2z'] - self.frame = frame - self.p1x = p1x - self.p1y = p1y - self.p1z = p1z - self.p2x = p2x - self.p2y = p2y - self.p2z = p2z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 108, struct.pack('>Bffffff', self.frame, self.p1x, self.p1y, self.p1z, self.p2x, self.p2y, self.p2z)) - -class MAVLink_set_roll_pitch_yaw_thrust_message(MAVLink_message): - ''' - Set roll, pitch and yaw. - ''' - def __init__(self, target_system, target_component, roll, pitch, yaw, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST, 'SET_ROLL_PITCH_YAW_THRUST') - self._fieldnames = ['target_system', 'target_component', 'roll', 'pitch', 'yaw', 'thrust'] - self.target_system = target_system - self.target_component = target_component - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 213, struct.pack('>BBffff', self.target_system, self.target_component, self.roll, self.pitch, self.yaw, self.thrust)) - -class MAVLink_set_roll_pitch_yaw_speed_thrust_message(MAVLink_message): - ''' - Set roll, pitch and yaw. - ''' - def __init__(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST, 'SET_ROLL_PITCH_YAW_SPEED_THRUST') - self._fieldnames = ['target_system', 'target_component', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust'] - self.target_system = target_system - self.target_component = target_component - self.roll_speed = roll_speed - self.pitch_speed = pitch_speed - self.yaw_speed = yaw_speed - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 95, struct.pack('>BBffff', self.target_system, self.target_component, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust)) - -class MAVLink_roll_pitch_yaw_thrust_setpoint_message(MAVLink_message): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - ''' - def __init__(self, time_us, roll, pitch, yaw, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, 'ROLL_PITCH_YAW_THRUST_SETPOINT') - self._fieldnames = ['time_us', 'roll', 'pitch', 'yaw', 'thrust'] - self.time_us = time_us - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 5, struct.pack('>Qffff', self.time_us, self.roll, self.pitch, self.yaw, self.thrust)) - -class MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(MAVLink_message): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active - on the system. - ''' - def __init__(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, 'ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT') - self._fieldnames = ['time_us', 'roll_speed', 'pitch_speed', 'yaw_speed', 'thrust'] - self.time_us = time_us - self.roll_speed = roll_speed - self.pitch_speed = pitch_speed - self.yaw_speed = yaw_speed - self.thrust = thrust - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 127, struct.pack('>Qffff', self.time_us, self.roll_speed, self.pitch_speed, self.yaw_speed, self.thrust)) - -class MAVLink_nav_controller_output_message(MAVLink_message): - ''' - Outputs of the APM navigation controller. The primary use of - this message is to check the response and signs of the - controller before actual flight and to assist with tuning - controller parameters - ''' - def __init__(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT, 'NAV_CONTROLLER_OUTPUT') - self._fieldnames = ['nav_roll', 'nav_pitch', 'nav_bearing', 'target_bearing', 'wp_dist', 'alt_error', 'aspd_error', 'xtrack_error'] - self.nav_roll = nav_roll - self.nav_pitch = nav_pitch - self.nav_bearing = nav_bearing - self.target_bearing = target_bearing - self.wp_dist = wp_dist - self.alt_error = alt_error - self.aspd_error = aspd_error - self.xtrack_error = xtrack_error - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 57, struct.pack('>ffhhHfff', self.nav_roll, self.nav_pitch, self.nav_bearing, self.target_bearing, self.wp_dist, self.alt_error, self.aspd_error, self.xtrack_error)) - -class MAVLink_position_target_message(MAVLink_message): - ''' - The goal position of the system. This position is the input to - any navigation or path planning algorithm and does NOT - represent the current controller setpoint. - ''' - def __init__(self, x, y, z, yaw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_POSITION_TARGET, 'POSITION_TARGET') - self._fieldnames = ['x', 'y', 'z', 'yaw'] - self.x = x - self.y = y - self.z = z - self.yaw = yaw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 126, struct.pack('>ffff', self.x, self.y, self.z, self.yaw)) - -class MAVLink_state_correction_message(MAVLink_message): - ''' - Corrects the systems state by adding an error correction term - to the position and velocity, and by rotating the attitude by - a correction angle. - ''' - def __init__(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATE_CORRECTION, 'STATE_CORRECTION') - self._fieldnames = ['xErr', 'yErr', 'zErr', 'rollErr', 'pitchErr', 'yawErr', 'vxErr', 'vyErr', 'vzErr'] - self.xErr = xErr - self.yErr = yErr - self.zErr = zErr - self.rollErr = rollErr - self.pitchErr = pitchErr - self.yawErr = yawErr - self.vxErr = vxErr - self.vyErr = vyErr - self.vzErr = vzErr - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 130, struct.pack('>fffffffff', self.xErr, self.yErr, self.zErr, self.rollErr, self.pitchErr, self.yawErr, self.vxErr, self.vyErr, self.vzErr)) - -class MAVLink_set_altitude_message(MAVLink_message): - ''' - - ''' - def __init__(self, target, mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_SET_ALTITUDE, 'SET_ALTITUDE') - self._fieldnames = ['target', 'mode'] - self.target = target - self.mode = mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 119, struct.pack('>BI', self.target, self.mode)) - -class MAVLink_request_data_stream_message(MAVLink_message): - ''' - - ''' - def __init__(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_REQUEST_DATA_STREAM, 'REQUEST_DATA_STREAM') - self._fieldnames = ['target_system', 'target_component', 'req_stream_id', 'req_message_rate', 'start_stop'] - self.target_system = target_system - self.target_component = target_component - self.req_stream_id = req_stream_id - self.req_message_rate = req_message_rate - self.start_stop = start_stop - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 193, struct.pack('>BBBHB', self.target_system, self.target_component, self.req_stream_id, self.req_message_rate, self.start_stop)) - -class MAVLink_hil_state_message(MAVLink_message): - ''' - This packet is useful for high throughput - applications such as hardware in the loop simulations. - ''' - def __init__(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_STATE, 'HIL_STATE') - self._fieldnames = ['usec', 'roll', 'pitch', 'yaw', 'rollspeed', 'pitchspeed', 'yawspeed', 'lat', 'lon', 'alt', 'vx', 'vy', 'vz', 'xacc', 'yacc', 'zacc'] - self.usec = usec - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.rollspeed = rollspeed - self.pitchspeed = pitchspeed - self.yawspeed = yawspeed - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - self.xacc = xacc - self.yacc = yacc - self.zacc = zacc - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 191, struct.pack('>Qffffffiiihhhhhh', self.usec, self.roll, self.pitch, self.yaw, self.rollspeed, self.pitchspeed, self.yawspeed, self.lat, self.lon, self.alt, self.vx, self.vy, self.vz, self.xacc, self.yacc, self.zacc)) - -class MAVLink_hil_controls_message(MAVLink_message): - ''' - Hardware in the loop control outputs - ''' - def __init__(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_HIL_CONTROLS, 'HIL_CONTROLS') - self._fieldnames = ['time_us', 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle', 'mode', 'nav_mode'] - self.time_us = time_us - self.roll_ailerons = roll_ailerons - self.pitch_elevator = pitch_elevator - self.yaw_rudder = yaw_rudder - self.throttle = throttle - self.mode = mode - self.nav_mode = nav_mode - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 236, struct.pack('>QffffBB', self.time_us, self.roll_ailerons, self.pitch_elevator, self.yaw_rudder, self.throttle, self.mode, self.nav_mode)) - -class MAVLink_manual_control_message(MAVLink_message): - ''' - - ''' - def __init__(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_MANUAL_CONTROL, 'MANUAL_CONTROL') - self._fieldnames = ['target', 'roll', 'pitch', 'yaw', 'thrust', 'roll_manual', 'pitch_manual', 'yaw_manual', 'thrust_manual'] - self.target = target - self.roll = roll - self.pitch = pitch - self.yaw = yaw - self.thrust = thrust - self.roll_manual = roll_manual - self.pitch_manual = pitch_manual - self.yaw_manual = yaw_manual - self.thrust_manual = thrust_manual - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 158, struct.pack('>BffffBBBB', self.target, self.roll, self.pitch, self.yaw, self.thrust, self.roll_manual, self.pitch_manual, self.yaw_manual, self.thrust_manual)) - -class MAVLink_rc_channels_override_message(MAVLink_message): - ''' - The RAW values of the RC channels sent to the MAV to override - info received from the RC radio. A value of -1 means no change - to that channel. A value of 0 means control of that channel - should be released back to the RC radio. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters might - violate this specification. - ''' - def __init__(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, 'RC_CHANNELS_OVERRIDE') - self._fieldnames = ['target_system', 'target_component', 'chan1_raw', 'chan2_raw', 'chan3_raw', 'chan4_raw', 'chan5_raw', 'chan6_raw', 'chan7_raw', 'chan8_raw'] - self.target_system = target_system - self.target_component = target_component - self.chan1_raw = chan1_raw - self.chan2_raw = chan2_raw - self.chan3_raw = chan3_raw - self.chan4_raw = chan4_raw - self.chan5_raw = chan5_raw - self.chan6_raw = chan6_raw - self.chan7_raw = chan7_raw - self.chan8_raw = chan8_raw - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 143, struct.pack('>BBHHHHHHHH', self.target_system, self.target_component, self.chan1_raw, self.chan2_raw, self.chan3_raw, self.chan4_raw, self.chan5_raw, self.chan6_raw, self.chan7_raw, self.chan8_raw)) - -class MAVLink_global_position_int_message(MAVLink_message): - ''' - The filtered global position (e.g. fused GPS and - accelerometers). The position is in GPS-frame (right-handed, - Z-up) - ''' - def __init__(self, lat, lon, alt, vx, vy, vz): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, 'GLOBAL_POSITION_INT') - self._fieldnames = ['lat', 'lon', 'alt', 'vx', 'vy', 'vz'] - self.lat = lat - self.lon = lon - self.alt = alt - self.vx = vx - self.vy = vy - self.vz = vz - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 104, struct.pack('>iiihhh', self.lat, self.lon, self.alt, self.vx, self.vy, self.vz)) - -class MAVLink_vfr_hud_message(MAVLink_message): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - ''' - def __init__(self, airspeed, groundspeed, heading, throttle, alt, climb): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_VFR_HUD, 'VFR_HUD') - self._fieldnames = ['airspeed', 'groundspeed', 'heading', 'throttle', 'alt', 'climb'] - self.airspeed = airspeed - self.groundspeed = groundspeed - self.heading = heading - self.throttle = throttle - self.alt = alt - self.climb = climb - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 123, struct.pack('>ffhHff', self.airspeed, self.groundspeed, self.heading, self.throttle, self.alt, self.climb)) - -class MAVLink_command_message(MAVLink_message): - ''' - Send a command with up to four parameters to the MAV - ''' - def __init__(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND, 'COMMAND') - self._fieldnames = ['target_system', 'target_component', 'command', 'confirmation', 'param1', 'param2', 'param3', 'param4'] - self.target_system = target_system - self.target_component = target_component - self.command = command - self.confirmation = confirmation - self.param1 = param1 - self.param2 = param2 - self.param3 = param3 - self.param4 = param4 - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 131, struct.pack('>BBBBffff', self.target_system, self.target_component, self.command, self.confirmation, self.param1, self.param2, self.param3, self.param4)) - -class MAVLink_command_ack_message(MAVLink_message): - ''' - Report status of a command. Includes feedback wether the - command was executed - ''' - def __init__(self, command, result): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_COMMAND_ACK, 'COMMAND_ACK') - self._fieldnames = ['command', 'result'] - self.command = command - self.result = result - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 8, struct.pack('>ff', self.command, self.result)) - -class MAVLink_optical_flow_message(MAVLink_message): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - ''' - def __init__(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_OPTICAL_FLOW, 'OPTICAL_FLOW') - self._fieldnames = ['time', 'sensor_id', 'flow_x', 'flow_y', 'quality', 'ground_distance'] - self.time = time - self.sensor_id = sensor_id - self.flow_x = flow_x - self.flow_y = flow_y - self.quality = quality - self.ground_distance = ground_distance - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 174, struct.pack('>QBhhBf', self.time, self.sensor_id, self.flow_x, self.flow_y, self.quality, self.ground_distance)) - -class MAVLink_object_detection_event_message(MAVLink_message): - ''' - Object has been detected - ''' - def __init__(self, time, object_id, type, name, quality, bearing, distance): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT, 'OBJECT_DETECTION_EVENT') - self._fieldnames = ['time', 'object_id', 'type', 'name', 'quality', 'bearing', 'distance'] - self.time = time - self.object_id = object_id - self.type = type - self.name = name - self.quality = quality - self.bearing = bearing - self.distance = distance - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 155, struct.pack('>IHB20sBff', self.time, self.object_id, self.type, self.name, self.quality, self.bearing, self.distance)) - -class MAVLink_debug_vect_message(MAVLink_message): - ''' - - ''' - def __init__(self, name, usec, x, y, z): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG_VECT, 'DEBUG_VECT') - self._fieldnames = ['name', 'usec', 'x', 'y', 'z'] - self.name = name - self.usec = usec - self.x = x - self.y = y - self.z = z - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 178, struct.pack('>10sQfff', self.name, self.usec, self.x, self.y, self.z)) - -class MAVLink_named_value_float_message(MAVLink_message): - ''' - Send a key-value pair as float. The use of this message is - discouraged for normal packets, but a quite efficient way for - testing new messages and getting experimental debug output. - ''' - def __init__(self, name, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 'NAMED_VALUE_FLOAT') - self._fieldnames = ['name', 'value'] - self.name = name - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 224, struct.pack('>10sf', self.name, self.value)) - -class MAVLink_named_value_int_message(MAVLink_message): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient way for - testing new messages and getting experimental debug output. - ''' - def __init__(self, name, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_NAMED_VALUE_INT, 'NAMED_VALUE_INT') - self._fieldnames = ['name', 'value'] - self.name = name - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 60, struct.pack('>10si', self.name, self.value)) - -class MAVLink_statustext_message(MAVLink_message): - ''' - Status text message. These messages are printed in yellow in - the COMM console of QGroundControl. WARNING: They consume - quite some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages are - buffered on the MCU and sent only at a limited rate (e.g. 10 - Hz). - ''' - def __init__(self, severity, text): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_STATUSTEXT, 'STATUSTEXT') - self._fieldnames = ['severity', 'text'] - self.severity = severity - self.text = text - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 106, struct.pack('>B50s', self.severity, self.text)) - -class MAVLink_debug_message(MAVLink_message): - ''' - Send a debug value. The index is used to discriminate between - values. These values show up in the plot of QGroundControl as - DEBUG N. - ''' - def __init__(self, ind, value): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_DEBUG, 'DEBUG') - self._fieldnames = ['ind', 'value'] - self.ind = ind - self.value = value - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 7, struct.pack('>Bf', self.ind, self.value)) - - -mavlink_map = { - MAVLINK_MSG_ID_SENSOR_OFFSETS : ( '>hhhfiiffffff', MAVLink_sensor_offsets_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11], 143 ), - MAVLINK_MSG_ID_SET_MAG_OFFSETS : ( '>BBhhh', MAVLink_set_mag_offsets_message, [0, 1, 2, 3, 4], 29 ), - MAVLINK_MSG_ID_MEMINFO : ( '>HH', MAVLink_meminfo_message, [0, 1], 208 ), - MAVLINK_MSG_ID_AP_ADC : ( '>HHHHHH', MAVLink_ap_adc_message, [0, 1, 2, 3, 4, 5], 188 ), - MAVLINK_MSG_ID_DIGICAM_CONFIGURE : ( '>BBBHBBBBBBf', MAVLink_digicam_configure_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10], 118 ), - MAVLINK_MSG_ID_DIGICAM_CONTROL : ( '>BBBBbBBBBf', MAVLink_digicam_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 242 ), - MAVLINK_MSG_ID_MOUNT_CONFIGURE : ( '>BBBBBB', MAVLink_mount_configure_message, [0, 1, 2, 3, 4, 5], 19 ), - MAVLINK_MSG_ID_MOUNT_CONTROL : ( '>BBiiiB', MAVLink_mount_control_message, [0, 1, 2, 3, 4, 5], 97 ), - MAVLINK_MSG_ID_MOUNT_STATUS : ( '>BBiii', MAVLink_mount_status_message, [0, 1, 2, 3, 4], 233 ), - MAVLINK_MSG_ID_FENCE_POINT : ( '>BBBBff', MAVLink_fence_point_message, [0, 1, 2, 3, 4, 5], 18 ), - MAVLINK_MSG_ID_FENCE_FETCH_POINT : ( '>BBB', MAVLink_fence_fetch_point_message, [0, 1, 2], 68 ), - MAVLINK_MSG_ID_FENCE_STATUS : ( '>BHBI', MAVLink_fence_status_message, [0, 1, 2, 3], 136 ), - MAVLINK_MSG_ID_AHRS : ( '>fffffff', MAVLink_ahrs_message, [0, 1, 2, 3, 4, 5, 6], 127 ), - MAVLINK_MSG_ID_SIMSTATE : ( '>fffffffff', MAVLink_simstate_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 42 ), - MAVLINK_MSG_ID_HWSTATUS : ( '>HB', MAVLink_hwstatus_message, [0, 1], 21 ), - MAVLINK_MSG_ID_RADIO : ( '>BBBBBHH', MAVLink_radio_message, [0, 1, 2, 3, 4, 5, 6], 93 ), - MAVLINK_MSG_ID_HEARTBEAT : ( '>BBB', MAVLink_heartbeat_message, [0, 1, 2], 72 ), - MAVLINK_MSG_ID_BOOT : ( '>I', MAVLink_boot_message, [0], 39 ), - MAVLINK_MSG_ID_SYSTEM_TIME : ( '>Q', MAVLink_system_time_message, [0], 190 ), - MAVLINK_MSG_ID_PING : ( '>IBBQ', MAVLink_ping_message, [0, 1, 2, 3], 92 ), - MAVLINK_MSG_ID_SYSTEM_TIME_UTC : ( '>II', MAVLink_system_time_utc_message, [0, 1], 191 ), - MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL : ( '>BBB25s', MAVLink_change_operator_control_message, [0, 1, 2, 3], 217 ), - MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK : ( '>BBB', MAVLink_change_operator_control_ack_message, [0, 1, 2], 104 ), - MAVLINK_MSG_ID_AUTH_KEY : ( '>32s', MAVLink_auth_key_message, [0], 119 ), - MAVLINK_MSG_ID_ACTION_ACK : ( '>BB', MAVLink_action_ack_message, [0, 1], 219 ), - MAVLINK_MSG_ID_ACTION : ( '>BBB', MAVLink_action_message, [0, 1, 2], 60 ), - MAVLINK_MSG_ID_SET_MODE : ( '>BB', MAVLink_set_mode_message, [0, 1], 186 ), - MAVLINK_MSG_ID_SET_NAV_MODE : ( '>BB', MAVLink_set_nav_mode_message, [0, 1], 10 ), - MAVLINK_MSG_ID_PARAM_REQUEST_READ : ( '>BB15sh', MAVLink_param_request_read_message, [0, 1, 2, 3], 89 ), - MAVLINK_MSG_ID_PARAM_REQUEST_LIST : ( '>BB', MAVLink_param_request_list_message, [0, 1], 159 ), - MAVLINK_MSG_ID_PARAM_VALUE : ( '>15sfHH', MAVLink_param_value_message, [0, 1, 2, 3], 162 ), - MAVLINK_MSG_ID_PARAM_SET : ( '>BB15sf', MAVLink_param_set_message, [0, 1, 2, 3], 121 ), - MAVLINK_MSG_ID_GPS_RAW_INT : ( '>QBiiiffff', MAVLink_gps_raw_int_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 149 ), - MAVLINK_MSG_ID_SCALED_IMU : ( '>Qhhhhhhhhh', MAVLink_scaled_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 222 ), - MAVLINK_MSG_ID_GPS_STATUS : ( '>B20s20s20s20s20s', MAVLink_gps_status_message, [0, 1, 2, 3, 4, 5], 110 ), - MAVLINK_MSG_ID_RAW_IMU : ( '>Qhhhhhhhhh', MAVLink_raw_imu_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 179 ), - MAVLINK_MSG_ID_RAW_PRESSURE : ( '>Qhhhh', MAVLink_raw_pressure_message, [0, 1, 2, 3, 4], 136 ), - MAVLINK_MSG_ID_SCALED_PRESSURE : ( '>Qffh', MAVLink_scaled_pressure_message, [0, 1, 2, 3], 229 ), - MAVLINK_MSG_ID_ATTITUDE : ( '>Qffffff', MAVLink_attitude_message, [0, 1, 2, 3, 4, 5, 6], 66 ), - MAVLINK_MSG_ID_LOCAL_POSITION : ( '>Qffffff', MAVLink_local_position_message, [0, 1, 2, 3, 4, 5, 6], 126 ), - MAVLINK_MSG_ID_GLOBAL_POSITION : ( '>Qffffff', MAVLink_global_position_message, [0, 1, 2, 3, 4, 5, 6], 147 ), - MAVLINK_MSG_ID_GPS_RAW : ( '>QBfffffff', MAVLink_gps_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 185 ), - MAVLINK_MSG_ID_SYS_STATUS : ( '>BBBHHHH', MAVLink_sys_status_message, [0, 1, 2, 3, 4, 5, 6], 112 ), - MAVLINK_MSG_ID_RC_CHANNELS_RAW : ( '>HHHHHHHHB', MAVLink_rc_channels_raw_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 252 ), - MAVLINK_MSG_ID_RC_CHANNELS_SCALED : ( '>hhhhhhhhB', MAVLink_rc_channels_scaled_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 162 ), - MAVLINK_MSG_ID_SERVO_OUTPUT_RAW : ( '>HHHHHHHH', MAVLink_servo_output_raw_message, [0, 1, 2, 3, 4, 5, 6, 7], 215 ), - MAVLINK_MSG_ID_WAYPOINT : ( '>BBHBBBBfffffff', MAVLink_waypoint_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13], 128 ), - MAVLINK_MSG_ID_WAYPOINT_REQUEST : ( '>BBH', MAVLink_waypoint_request_message, [0, 1, 2], 9 ), - MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT : ( '>BBH', MAVLink_waypoint_set_current_message, [0, 1, 2], 106 ), - MAVLINK_MSG_ID_WAYPOINT_CURRENT : ( '>H', MAVLink_waypoint_current_message, [0], 101 ), - MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST : ( '>BB', MAVLink_waypoint_request_list_message, [0, 1], 213 ), - MAVLINK_MSG_ID_WAYPOINT_COUNT : ( '>BBH', MAVLink_waypoint_count_message, [0, 1, 2], 4 ), - MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL : ( '>BB', MAVLink_waypoint_clear_all_message, [0, 1], 229 ), - MAVLINK_MSG_ID_WAYPOINT_REACHED : ( '>H', MAVLink_waypoint_reached_message, [0], 21 ), - MAVLINK_MSG_ID_WAYPOINT_ACK : ( '>BBB', MAVLink_waypoint_ack_message, [0, 1, 2], 214 ), - MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN : ( '>BBiii', MAVLink_gps_set_global_origin_message, [0, 1, 2, 3, 4], 215 ), - MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET : ( '>iii', MAVLink_gps_local_origin_set_message, [0, 1, 2], 14 ), - MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET : ( '>BBffff', MAVLink_local_position_setpoint_set_message, [0, 1, 2, 3, 4, 5], 206 ), - MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT : ( '>ffff', MAVLink_local_position_setpoint_message, [0, 1, 2, 3], 50 ), - MAVLINK_MSG_ID_CONTROL_STATUS : ( '>BBBBBBBB', MAVLink_control_status_message, [0, 1, 2, 3, 4, 5, 6, 7], 157 ), - MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA : ( '>BBBffffff', MAVLink_safety_set_allowed_area_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 126 ), - MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA : ( '>Bffffff', MAVLink_safety_allowed_area_message, [0, 1, 2, 3, 4, 5, 6], 108 ), - MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_thrust_message, [0, 1, 2, 3, 4, 5], 213 ), - MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_THRUST : ( '>BBffff', MAVLink_set_roll_pitch_yaw_speed_thrust_message, [0, 1, 2, 3, 4, 5], 95 ), - MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_thrust_setpoint_message, [0, 1, 2, 3, 4], 5 ), - MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT : ( '>Qffff', MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message, [0, 1, 2, 3, 4], 127 ), - MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT : ( '>ffhhHfff', MAVLink_nav_controller_output_message, [0, 1, 2, 3, 4, 5, 6, 7], 57 ), - MAVLINK_MSG_ID_POSITION_TARGET : ( '>ffff', MAVLink_position_target_message, [0, 1, 2, 3], 126 ), - MAVLINK_MSG_ID_STATE_CORRECTION : ( '>fffffffff', MAVLink_state_correction_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 130 ), - MAVLINK_MSG_ID_SET_ALTITUDE : ( '>BI', MAVLink_set_altitude_message, [0, 1], 119 ), - MAVLINK_MSG_ID_REQUEST_DATA_STREAM : ( '>BBBHB', MAVLink_request_data_stream_message, [0, 1, 2, 3, 4], 193 ), - MAVLINK_MSG_ID_HIL_STATE : ( '>Qffffffiiihhhhhh', MAVLink_hil_state_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15], 191 ), - MAVLINK_MSG_ID_HIL_CONTROLS : ( '>QffffBB', MAVLink_hil_controls_message, [0, 1, 2, 3, 4, 5, 6], 236 ), - MAVLINK_MSG_ID_MANUAL_CONTROL : ( '>BffffBBBB', MAVLink_manual_control_message, [0, 1, 2, 3, 4, 5, 6, 7, 8], 158 ), - MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE : ( '>BBHHHHHHHH', MAVLink_rc_channels_override_message, [0, 1, 2, 3, 4, 5, 6, 7, 8, 9], 143 ), - MAVLINK_MSG_ID_GLOBAL_POSITION_INT : ( '>iiihhh', MAVLink_global_position_int_message, [0, 1, 2, 3, 4, 5], 104 ), - MAVLINK_MSG_ID_VFR_HUD : ( '>ffhHff', MAVLink_vfr_hud_message, [0, 1, 2, 3, 4, 5], 123 ), - MAVLINK_MSG_ID_COMMAND : ( '>BBBBffff', MAVLink_command_message, [0, 1, 2, 3, 4, 5, 6, 7], 131 ), - MAVLINK_MSG_ID_COMMAND_ACK : ( '>ff', MAVLink_command_ack_message, [0, 1], 8 ), - MAVLINK_MSG_ID_OPTICAL_FLOW : ( '>QBhhBf', MAVLink_optical_flow_message, [0, 1, 2, 3, 4, 5], 174 ), - MAVLINK_MSG_ID_OBJECT_DETECTION_EVENT : ( '>IHB20sBff', MAVLink_object_detection_event_message, [0, 1, 2, 3, 4, 5, 6], 155 ), - MAVLINK_MSG_ID_DEBUG_VECT : ( '>10sQfff', MAVLink_debug_vect_message, [0, 1, 2, 3, 4], 178 ), - MAVLINK_MSG_ID_NAMED_VALUE_FLOAT : ( '>10sf', MAVLink_named_value_float_message, [0, 1], 224 ), - MAVLINK_MSG_ID_NAMED_VALUE_INT : ( '>10si', MAVLink_named_value_int_message, [0, 1], 60 ), - MAVLINK_MSG_ID_STATUSTEXT : ( '>B50s', MAVLink_statustext_message, [0, 1], 106 ), - MAVLINK_MSG_ID_DEBUG : ( '>Bf', MAVLink_debug_message, [0, 1], 7 ), -} - -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 = 85 - self.little_endian = False - self.crc_extra = False - self.sort_fields = False - 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] != 85: - 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) != 85: - 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(' MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng) - msg.pack(self) - return msg - - def fence_point_send(self, target_system, target_component, idx, count, lat, lng): - ''' - A fence point. Used to set a point when from GCS -> MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng)) - - def fence_fetch_point_encode(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx) - msg.pack(self) - return msg - - def fence_fetch_point_send(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - return self.send(self.fence_fetch_point_encode(target_system, target_component, idx)) - - def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time) - msg.pack(self) - return msg - - def fence_status_send(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time)) - - def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) - msg.pack(self) - return msg - - def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - - def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) - msg.pack(self) - return msg - - def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - - def hwstatus_encode(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - msg = MAVLink_hwstatus_message(Vcc, I2Cerr) - msg.pack(self) - return msg - - def hwstatus_send(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - - def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) - msg.pack(self) - return msg - - def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - - def heartbeat_encode(self, type, autopilot, mavlink_version=2): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - msg = MAVLink_heartbeat_message(type, autopilot, mavlink_version) - msg.pack(self) - return msg - - def heartbeat_send(self, type, autopilot, mavlink_version=2): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM (uint8_t) - mavlink_version : MAVLink version (uint8_t) - - ''' - return self.send(self.heartbeat_encode(type, autopilot, mavlink_version)) - - def boot_encode(self, version): - ''' - The boot message indicates that a system is starting. The onboard - software version allows to keep track of onboard - soft/firmware revisions. - - version : The onboard software version (uint32_t) - - ''' - msg = MAVLink_boot_message(version) - msg.pack(self) - return msg - - def boot_send(self, version): - ''' - The boot message indicates that a system is starting. The onboard - software version allows to keep track of onboard - soft/firmware revisions. - - version : The onboard software version (uint32_t) - - ''' - return self.send(self.boot_encode(version)) - - def system_time_encode(self, time_usec): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - - ''' - msg = MAVLink_system_time_message(time_usec) - msg.pack(self) - return msg - - def system_time_send(self, time_usec): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - - ''' - return self.send(self.system_time_encode(time_usec)) - - def ping_encode(self, seq, target_system, target_component, time): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - time : Unix timestamp in microseconds (uint64_t) - - ''' - msg = MAVLink_ping_message(seq, target_system, target_component, time) - msg.pack(self) - return msg - - def ping_send(self, seq, target_system, target_component, time): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - time : Unix timestamp in microseconds (uint64_t) - - ''' - return self.send(self.ping_encode(seq, target_system, target_component, time)) - - def system_time_utc_encode(self, utc_date, utc_time): - ''' - UTC date and time from GPS module - - utc_date : GPS UTC date ddmmyy (uint32_t) - utc_time : GPS UTC time hhmmss (uint32_t) - - ''' - msg = MAVLink_system_time_utc_message(utc_date, utc_time) - msg.pack(self) - return msg - - def system_time_utc_send(self, utc_date, utc_time): - ''' - UTC date and time from GPS module - - utc_date : GPS UTC date ddmmyy (uint32_t) - utc_time : GPS UTC time hhmmss (uint32_t) - - ''' - return self.send(self.system_time_utc_encode(utc_date, utc_time)) - - def change_operator_control_encode(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey) - msg.pack(self) - return msg - - def change_operator_control_send(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey)) - - def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack) - msg.pack(self) - return msg - - def change_operator_control_ack_send(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack)) - - def auth_key_encode(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - msg = MAVLink_auth_key_message(key) - msg.pack(self) - return msg - - def auth_key_send(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - return self.send(self.auth_key_encode(key)) - - def action_ack_encode(self, action, result): - ''' - This message acknowledges an action. IMPORTANT: The acknowledgement - can be also negative, e.g. the MAV rejects a reset - message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in - mavlink/include/mavlink_types.h - - action : The action id (uint8_t) - result : 0: Action DENIED, 1: Action executed (uint8_t) - - ''' - msg = MAVLink_action_ack_message(action, result) - msg.pack(self) - return msg - - def action_ack_send(self, action, result): - ''' - This message acknowledges an action. IMPORTANT: The acknowledgement - can be also negative, e.g. the MAV rejects a reset - message because it is in-flight. The action ids are - defined in ENUM MAV_ACTION in - mavlink/include/mavlink_types.h - - action : The action id (uint8_t) - result : 0: Action DENIED, 1: Action executed (uint8_t) - - ''' - return self.send(self.action_ack_encode(action, result)) - - def action_encode(self, target, target_component, action): - ''' - An action message allows to execute a certain onboard action. These - include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM - MAV_ACTION in mavlink/include/mavlink_types.h - - target : The system executing the action (uint8_t) - target_component : The component executing the action (uint8_t) - action : The action id (uint8_t) - - ''' - msg = MAVLink_action_message(target, target_component, action) - msg.pack(self) - return msg - - def action_send(self, target, target_component, action): - ''' - An action message allows to execute a certain onboard action. These - include liftoff, land, storing parameters too EEPROM, - shutddown, etc. The action ids are defined in ENUM - MAV_ACTION in mavlink/include/mavlink_types.h - - target : The system executing the action (uint8_t) - target_component : The component executing the action (uint8_t) - action : The action id (uint8_t) - - ''' - return self.send(self.action_encode(target, target_component, action)) - - def set_mode_encode(self, target, mode): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target : The system setting the mode (uint8_t) - mode : The new mode (uint8_t) - - ''' - msg = MAVLink_set_mode_message(target, mode) - msg.pack(self) - return msg - - def set_mode_send(self, target, mode): - ''' - Set the system mode, as defined by enum MAV_MODE in - mavlink/include/mavlink_types.h. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target : The system setting the mode (uint8_t) - mode : The new mode (uint8_t) - - ''' - return self.send(self.set_mode_encode(target, mode)) - - def set_nav_mode_encode(self, target, nav_mode): - ''' - Set the system navigation mode, as defined by enum MAV_NAV_MODE in - mavlink/include/mavlink_types.h. The navigation mode - applies to the whole aircraft and thus all components. - - target : The system setting the mode (uint8_t) - nav_mode : The new navigation mode (uint8_t) - - ''' - msg = MAVLink_set_nav_mode_message(target, nav_mode) - msg.pack(self) - return msg - - def set_nav_mode_send(self, target, nav_mode): - ''' - Set the system navigation mode, as defined by enum MAV_NAV_MODE in - mavlink/include/mavlink_types.h. The navigation mode - applies to the whole aircraft and thus all components. - - target : The system setting the mode (uint8_t) - nav_mode : The new navigation mode (uint8_t) - - ''' - return self.send(self.set_nav_mode_encode(target, nav_mode)) - - def param_request_read_encode(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index) - msg.pack(self) - return msg - - def param_request_read_send(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_index : Parameter index. Send -1 to use the param ID field as identifier (int16_t) - - ''' - return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index)) - - def param_request_list_encode(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_param_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def param_request_list_send(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.param_request_list_encode(target_system, target_component)) - - def param_value_encode(self, param_id, param_value, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - msg = MAVLink_param_value_message(param_id, param_value, param_count, param_index) - msg.pack(self) - return msg - - def param_value_send(self, param_id, param_value, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - return self.send(self.param_value_encode(param_id, param_value, param_count, param_index)) - - def param_set_encode(self, target_system, target_component, param_id, param_value): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - - ''' - msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value) - msg.pack(self) - return msg - - def param_set_send(self, target_system, target_component, param_id, param_value): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id (int8_t) - param_value : Onboard parameter value (float) - - ''' - return self.send(self.param_set_encode(target_system, target_component, param_id, param_value)) - - def gps_raw_int_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) (int32_t) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (m/s) (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - msg = MAVLink_gps_raw_int_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg) - msg.pack(self) - return msg - - def gps_raw_int_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) (int32_t) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (m/s) (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - return self.send(self.gps_raw_int_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)) - - def scaled_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - msg = MAVLink_scaled_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def scaled_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - return self.send(self.scaled_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (int8_t) - satellite_used : 0: Satellite not used, 1: used for localization (int8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t) - satellite_snr : Signal to noise ratio of satellite (int8_t) - - ''' - msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr) - msg.pack(self) - return msg - - def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (int8_t) - satellite_used : 0: Satellite not used, 1: used for localization (int8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (int8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (int8_t) - satellite_snr : Signal to noise ratio of satellite (int8_t) - - ''' - return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)) - - def raw_imu_encode(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - msg = MAVLink_raw_imu_message(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def raw_imu_send(self, usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - return self.send(self.raw_imu_encode(usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_pressure_encode(self, usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - msg = MAVLink_raw_pressure_message(usec, press_abs, press_diff1, press_diff2, temperature) - msg.pack(self) - return msg - - def raw_pressure_send(self, usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - return self.send(self.raw_pressure_encode(usec, press_abs, press_diff1, press_diff2, temperature)) - - def scaled_pressure_encode(self, usec, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - msg = MAVLink_scaled_pressure_message(usec, press_abs, press_diff, temperature) - msg.pack(self) - return msg - - def scaled_pressure_send(self, usec, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - return self.send(self.scaled_pressure_encode(usec, press_abs, press_diff, temperature)) - - def attitude_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)) - - def local_position_encode(self, usec, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - msg = MAVLink_local_position_message(usec, x, y, z, vx, vy, vz) - msg.pack(self) - return msg - - def local_position_send(self, usec, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - return self.send(self.local_position_encode(usec, x, y, z, vx, vy, vz)) - - def global_position_encode(self, usec, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since unix epoch) (uint64_t) - lat : Latitude, in degrees (float) - lon : Longitude, in degrees (float) - alt : Absolute altitude, in meters (float) - vx : X Speed (in Latitude direction, positive: going north) (float) - vy : Y Speed (in Longitude direction, positive: going east) (float) - vz : Z Speed (in Altitude direction, positive: going up) (float) - - ''' - msg = MAVLink_global_position_message(usec, lat, lon, alt, vx, vy, vz) - msg.pack(self) - return msg - - def global_position_send(self, usec, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since unix epoch) (uint64_t) - lat : Latitude, in degrees (float) - lon : Longitude, in degrees (float) - alt : Absolute altitude, in meters (float) - vx : X Speed (in Latitude direction, positive: going north) (float) - vy : Y Speed (in Longitude direction, positive: going east) (float) - vz : Z Speed (in Altitude direction, positive: going up) (float) - - ''' - return self.send(self.global_position_encode(usec, lat, lon, alt, vx, vy, vz)) - - def gps_raw_encode(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in degrees (float) - lon : Longitude in degrees (float) - alt : Altitude in meters (float) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - msg = MAVLink_gps_raw_message(usec, fix_type, lat, lon, alt, eph, epv, v, hdg) - msg.pack(self) - return msg - - def gps_raw_send(self, usec, fix_type, lat, lon, alt, eph, epv, v, hdg): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position estimate of the - sytem, but rather a RAW sensor value. See message - GLOBAL_POSITION for the global position estimate. - Coordinate frame is right-handed, Z-axis up (GPS - frame) - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in degrees (float) - lon : Longitude in degrees (float) - alt : Altitude in meters (float) - eph : GPS HDOP (float) - epv : GPS VDOP (float) - v : GPS ground speed (float) - hdg : Compass heading in degrees, 0..360 degrees (float) - - ''' - return self.send(self.gps_raw_encode(usec, fix_type, lat, lon, alt, eph, epv, v, hdg)) - - def sys_status_encode(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - mode : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t) - nav_mode : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t) - status : System status flag, see MAV_STATUS ENUM (uint8_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - vbat : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t) - packet_drop : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t) - - ''' - msg = MAVLink_sys_status_message(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop) - msg.pack(self) - return msg - - def sys_status_send(self, mode, nav_mode, status, load, vbat, battery_remaining, packet_drop): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - mode : System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h (uint8_t) - nav_mode : Navigation mode, see MAV_NAV_MODE ENUM (uint8_t) - status : System status flag, see MAV_STATUS ENUM (uint8_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - vbat : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 1000) (uint16_t) - packet_drop : Dropped packets (packets that were corrupted on reception on the MAV) (uint16_t) - - ''' - return self.send(self.sys_status_encode(mode, nav_mode, status, load, vbat, battery_remaining, packet_drop)) - - def rc_channels_raw_encode(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_raw_message(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi) - msg.pack(self) - return msg - - def rc_channels_raw_send(self, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_raw_encode(chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)) - - def rc_channels_scaled_encode(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_scaled_message(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi) - msg.pack(self) - return msg - - def rc_channels_scaled_send(self, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_scaled_encode(chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)) - - def servo_output_raw_encode(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_servo_output_raw_message(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw) - msg.pack(self) - return msg - - def servo_output_raw_send(self, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.servo_output_raw_encode(servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)) - - def waypoint_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on - the system. The waypoint can be either in x, y, z - meters (type: LOCAL) or x:lat, y:lon, z:altitude. - Local frame is Z-down, right handed, global frame is - Z-up, right handed - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - msg = MAVLink_waypoint_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z) - msg.pack(self) - return msg - - def waypoint_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on - the system. The waypoint can be either in x, y, z - meters (type: LOCAL) or x:lat, y:lon, z:altitude. - Local frame is Z-down, right handed, global frame is - Z-up, right handed - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs (uint8_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - return self.send(self.waypoint_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)) - - def waypoint_request_encode(self, target_system, target_component, seq): - ''' - Request the information of the waypoint with the sequence number seq. - The response of the system to this message should be a - WAYPOINT message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_request_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def waypoint_request_send(self, target_system, target_component, seq): - ''' - Request the information of the waypoint with the sequence number seq. - The response of the system to this message should be a - WAYPOINT message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_request_encode(target_system, target_component, seq)) - - def waypoint_set_current_encode(self, target_system, target_component, seq): - ''' - Set the waypoint with sequence number seq as current waypoint. This - means that the MAV will continue to this waypoint on - the shortest path (not following the waypoints in- - between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_set_current_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def waypoint_set_current_send(self, target_system, target_component, seq): - ''' - Set the waypoint with sequence number seq as current waypoint. This - means that the MAV will continue to this waypoint on - the shortest path (not following the waypoints in- - between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_set_current_encode(target_system, target_component, seq)) - - def waypoint_current_encode(self, seq): - ''' - Message that announces the sequence number of the current active - waypoint. The MAV will fly towards this waypoint. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_current_message(seq) - msg.pack(self) - return msg - - def waypoint_current_send(self, seq): - ''' - Message that announces the sequence number of the current active - waypoint. The MAV will fly towards this waypoint. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_current_encode(seq)) - - def waypoint_request_list_encode(self, target_system, target_component): - ''' - Request the overall list of waypoints from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_waypoint_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def waypoint_request_list_send(self, target_system, target_component): - ''' - Request the overall list of waypoints from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.waypoint_request_list_encode(target_system, target_component)) - - def waypoint_count_encode(self, target_system, target_component, count): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST by the - MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of - waypoints. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of Waypoints in the Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_count_message(target_system, target_component, count) - msg.pack(self) - return msg - - def waypoint_count_send(self, target_system, target_component, count): - ''' - This message is emitted as response to WAYPOINT_REQUEST_LIST by the - MAV. The GCS can then request the individual waypoints - based on the knowledge of the total number of - waypoints. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of Waypoints in the Sequence (uint16_t) - - ''' - return self.send(self.waypoint_count_encode(target_system, target_component, count)) - - def waypoint_clear_all_encode(self, target_system, target_component): - ''' - Delete all waypoints at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_waypoint_clear_all_message(target_system, target_component) - msg.pack(self) - return msg - - def waypoint_clear_all_send(self, target_system, target_component): - ''' - Delete all waypoints at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.waypoint_clear_all_encode(target_system, target_component)) - - def waypoint_reached_encode(self, seq): - ''' - A certain waypoint has been reached. The system will either hold this - position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - waypoint. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_waypoint_reached_message(seq) - msg.pack(self) - return msg - - def waypoint_reached_send(self, seq): - ''' - A certain waypoint has been reached. The system will either hold this - position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - waypoint. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.waypoint_reached_encode(seq)) - - def waypoint_ack_encode(self, target_system, target_component, type): - ''' - Ack message during waypoint handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : 0: OK, 1: Error (uint8_t) - - ''' - msg = MAVLink_waypoint_ack_message(target_system, target_component, type) - msg.pack(self) - return msg - - def waypoint_ack_send(self, target_system, target_component, type): - ''' - Ack message during waypoint handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : 0: OK, 1: Error (uint8_t) - - ''' - return self.send(self.waypoint_ack_encode(target_system, target_component, type)) - - def gps_set_global_origin_encode(self, target_system, target_component, latitude, longitude, altitude): - ''' - As local waypoints exist, the global waypoint reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - msg = MAVLink_gps_set_global_origin_message(target_system, target_component, latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_set_global_origin_send(self, target_system, target_component, latitude, longitude, altitude): - ''' - As local waypoints exist, the global waypoint reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - return self.send(self.gps_set_global_origin_encode(target_system, target_component, latitude, longitude, altitude)) - - def gps_local_origin_set_encode(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - msg = MAVLink_gps_local_origin_set_message(latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_local_origin_set_send(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - return self.send(self.gps_local_origin_set_encode(latitude, longitude, altitude)) - - def local_position_setpoint_set_encode(self, target_system, target_component, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_set_message(target_system, target_component, x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_set_send(self, target_system, target_component, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/waypoint planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_set_encode(target_system, target_component, x, y, z, yaw)) - - def local_position_setpoint_encode(self, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_message(x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_send(self, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_encode(x, y, z, yaw)) - - def control_status_encode(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - ''' - - - position_fix : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t) - vision_fix : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t) - gps_fix : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t) - ahrs_health : Attitude estimation health: 0: poor, 255: excellent (uint8_t) - control_att : 0: Attitude control disabled, 1: enabled (uint8_t) - control_pos_xy : 0: X, Y position control disabled, 1: enabled (uint8_t) - control_pos_z : 0: Z position control disabled, 1: enabled (uint8_t) - control_pos_yaw : 0: Yaw angle control disabled, 1: enabled (uint8_t) - - ''' - msg = MAVLink_control_status_message(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw) - msg.pack(self) - return msg - - def control_status_send(self, position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw): - ''' - - - position_fix : Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix (uint8_t) - vision_fix : Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix (uint8_t) - gps_fix : GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix (uint8_t) - ahrs_health : Attitude estimation health: 0: poor, 255: excellent (uint8_t) - control_att : 0: Attitude control disabled, 1: enabled (uint8_t) - control_pos_xy : 0: X, Y position control disabled, 1: enabled (uint8_t) - control_pos_z : 0: Z position control disabled, 1: enabled (uint8_t) - control_pos_yaw : 0: Yaw angle control disabled, 1: enabled (uint8_t) - - ''' - return self.send(self.control_status_encode(position_fix, vision_fix, gps_fix, ahrs_health, control_att, control_pos_xy, control_pos_z, control_pos_yaw)) - - def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/waypoints to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust)) - - def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)) - - def roll_pitch_yaw_thrust_setpoint_encode(self, time_us, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_us, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_thrust_setpoint_send(self, time_us, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_us, roll, pitch, yaw, thrust)) - - def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_us, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_us, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_us : Timestamp in micro seconds since unix epoch (uint64_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_us, roll_speed, pitch_speed, yaw_speed, thrust)) - - def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current waypoint/target in degrees (int16_t) - wp_dist : Distance to active waypoint in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error) - msg.pack(self) - return msg - - def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current waypoint/target in degrees (int16_t) - wp_dist : Distance to active waypoint in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)) - - def position_target_encode(self, x, y, z, yaw): - ''' - The goal position of the system. This position is the input to any - navigation or path planning algorithm and does NOT - represent the current controller setpoint. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : yaw orientation in radians, 0 = NORTH (float) - - ''' - msg = MAVLink_position_target_message(x, y, z, yaw) - msg.pack(self) - return msg - - def position_target_send(self, x, y, z, yaw): - ''' - The goal position of the system. This position is the input to any - navigation or path planning algorithm and does NOT - represent the current controller setpoint. - - x : x position (float) - y : y position (float) - z : z position (float) - yaw : yaw orientation in radians, 0 = NORTH (float) - - ''' - return self.send(self.position_target_encode(x, y, z, yaw)) - - def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr) - msg.pack(self) - return msg - - def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)) - - def set_altitude_encode(self, target, mode): - ''' - - - target : The system setting the altitude (uint8_t) - mode : The new altitude in meters (uint32_t) - - ''' - msg = MAVLink_set_altitude_message(target, mode) - msg.pack(self) - return msg - - def set_altitude_send(self, target, mode): - ''' - - - target : The system setting the altitude (uint8_t) - mode : The new altitude in meters (uint32_t) - - ''' - return self.send(self.set_altitude_encode(target, mode)) - - def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested message type (uint8_t) - req_message_rate : Update rate in Hertz (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop) - msg.pack(self) - return msg - - def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested message type (uint8_t) - req_message_rate : Update rate in Hertz (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop)) - - def hil_state_encode(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - This packet is useful for high throughput applications - such as hardware in the loop simulations. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - msg = MAVLink_hil_state_message(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc) - msg.pack(self) - return msg - - def hil_state_send(self, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - This packet is useful for high throughput applications - such as hardware in the loop simulations. - - usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - return self.send(self.hil_state_encode(usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)) - - def hil_controls_encode(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - ''' - Hardware in the loop control outputs - - time_us : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -3 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - msg = MAVLink_hil_controls_message(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode) - msg.pack(self) - return msg - - def hil_controls_send(self, time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode): - ''' - Hardware in the loop control outputs - - time_us : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -3 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - return self.send(self.hil_controls_encode(time_us, roll_ailerons, pitch_elevator, yaw_rudder, throttle, mode, nav_mode)) - - def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual) - msg.pack(self) - return msg - - def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)) - - def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw) - msg.pack(self) - return msg - - def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)) - - def global_position_int_encode(self, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up) - - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - - ''' - msg = MAVLink_global_position_int_message(lat, lon, alt, vx, vy, vz) - msg.pack(self) - return msg - - def global_position_int_send(self, lat, lon, alt, vx, vy, vz): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up) - - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - - ''' - return self.send(self.global_position_int_encode(lat, lon, alt, vx, vy, vz)) - - def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb) - msg.pack(self) - return msg - - def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb)) - - def command_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint8_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - - ''' - msg = MAVLink_command_message(target_system, target_component, command, confirmation, param1, param2, param3, param4) - msg.pack(self) - return msg - - def command_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint8_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - - ''' - return self.send(self.command_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4)) - - def command_ack_encode(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Current airspeed in m/s (float) - result : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float) - - ''' - msg = MAVLink_command_ack_message(command, result) - msg.pack(self) - return msg - - def command_ack_send(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed - - command : Current airspeed in m/s (float) - result : 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION (float) - - ''' - return self.send(self.command_ack_encode(command, result)) - - def optical_flow_encode(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - msg = MAVLink_optical_flow_message(time, sensor_id, flow_x, flow_y, quality, ground_distance) - msg.pack(self) - return msg - - def optical_flow_send(self, time, sensor_id, flow_x, flow_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters (float) - - ''' - return self.send(self.optical_flow_encode(time, sensor_id, flow_x, flow_y, quality, ground_distance)) - - def object_detection_event_encode(self, time, object_id, type, name, quality, bearing, distance): - ''' - Object has been detected - - time : Timestamp in milliseconds since system boot (uint32_t) - object_id : Object ID (uint16_t) - type : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t) - name : Name of the object as defined by the detector (char) - quality : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t) - bearing : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float) - distance : Ground distance in meters (float) - - ''' - msg = MAVLink_object_detection_event_message(time, object_id, type, name, quality, bearing, distance) - msg.pack(self) - return msg - - def object_detection_event_send(self, time, object_id, type, name, quality, bearing, distance): - ''' - Object has been detected - - time : Timestamp in milliseconds since system boot (uint32_t) - object_id : Object ID (uint16_t) - type : Object type: 0: image, 1: letter, 2: ground vehicle, 3: air vehicle, 4: surface vehicle, 5: sub-surface vehicle, 6: human, 7: animal (uint8_t) - name : Name of the object as defined by the detector (char) - quality : Detection quality / confidence. 0: bad, 255: maximum confidence (uint8_t) - bearing : Angle of the object with respect to the body frame in NED coordinates in radians. 0: front (float) - distance : Ground distance in meters (float) - - ''' - return self.send(self.object_detection_event_encode(time, object_id, type, name, quality, bearing, distance)) - - def debug_vect_encode(self, name, usec, x, y, z): - ''' - - - name : Name (char) - usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - msg = MAVLink_debug_vect_message(name, usec, x, y, z) - msg.pack(self) - return msg - - def debug_vect_send(self, name, usec, x, y, z): - ''' - - - name : Name (char) - usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - return self.send(self.debug_vect_encode(name, usec, x, y, z)) - - def named_value_float_encode(self, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - msg = MAVLink_named_value_float_message(name, value) - msg.pack(self) - return msg - - def named_value_float_send(self, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - return self.send(self.named_value_float_encode(name, value)) - - def named_value_int_encode(self, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - msg = MAVLink_named_value_int_message(name, value) - msg.pack(self) - return msg - - def named_value_int_send(self, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - return self.send(self.named_value_int_encode(name, value)) - - def statustext_encode(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (int8_t) - - ''' - msg = MAVLink_statustext_message(severity, text) - msg.pack(self) - return msg - - def statustext_send(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status, 0 = info message, 255 = critical fault (uint8_t) - text : Status text message, without null termination character (int8_t) - - ''' - return self.send(self.statustext_encode(severity, text)) - - def debug_encode(self, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - msg = MAVLink_debug_message(ind, value) - msg.pack(self) - return msg - - def debug_send(self, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - return self.send(self.debug_encode(ind, value)) - diff --git a/Tools/autotest/pymavlink/mavlinkv10.py b/Tools/autotest/pymavlink/mavlinkv10.py deleted file mode 100644 index 9c6c055c0f..0000000000 --- a/Tools/autotest/pymavlink/mavlinkv10.py +++ /dev/null @@ -1,5602 +0,0 @@ -''' -MAVLink protocol implementation (auto-generated by mavgen.py) - -Generated from: ardupilotmega.xml,common.xml - -Note: this file has been auto-generated. DO NOT EDIT -''' - -import struct, array, mavutil, time - -WIRE_PROTOCOL_VERSION = "1.0" - - -# some base types from mavlink_types.h -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 - - -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', 254, 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): - if isinstance(self._msgbuf, str): - return self._msgbuf - return self._msgbuf.tostring() - - 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 True: # using CRC extra - crc.accumulate(chr(crc_extra)) - self._crc = crc.crc - self._msgbuf += struct.pack(' MAV. Also used to return a point from MAV -> GCS - ''' - def __init__(self, target_system, target_component, idx, count, lat, lng): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_FENCE_POINT, 'FENCE_POINT') - self._fieldnames = ['target_system', 'target_component', 'idx', 'count', 'lat', 'lng'] - self.target_system = target_system - self.target_component = target_component - self.idx = idx - self.count = count - self.lat = lat - self.lng = lng - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 78, struct.pack(' - value[float]. This allows to send a parameter to any other - component (such as the GCS) without the need of previous - knowledge of possible parameter names. Thus the same GCS can - store different parameters for different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a full - documentation of QGroundControl and IMU code. - ''' - def __init__(self, target_system, target_component, param_id, param_index): - MAVLink_message.__init__(self, MAVLINK_MSG_ID_PARAM_REQUEST_READ, 'PARAM_REQUEST_READ') - self._fieldnames = ['target_system', 'target_component', 'param_id', 'param_index'] - self.target_system = target_system - self.target_component = target_component - self.param_id = param_id - self.param_index = param_index - - def pack(self, mav): - return MAVLink_message.pack(self, mav, 214, struct.pack('= 1 and self.buf[0] != 254: - 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) != 254: - 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(' MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - msg = MAVLink_fence_point_message(target_system, target_component, idx, count, lat, lng) - msg.pack(self) - return msg - - def fence_point_send(self, target_system, target_component, idx, count, lat, lng): - ''' - A fence point. Used to set a point when from GCS -> MAV. - Also used to return a point from MAV -> GCS - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - count : total number of points (for sanity checking) (uint8_t) - lat : Latitude of point (float) - lng : Longitude of point (float) - - ''' - return self.send(self.fence_point_encode(target_system, target_component, idx, count, lat, lng)) - - def fence_fetch_point_encode(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - msg = MAVLink_fence_fetch_point_message(target_system, target_component, idx) - msg.pack(self) - return msg - - def fence_fetch_point_send(self, target_system, target_component, idx): - ''' - Request a current fence point from MAV - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - idx : point index (first point is 1, 0 is for return point) (uint8_t) - - ''' - return self.send(self.fence_fetch_point_encode(target_system, target_component, idx)) - - def fence_status_encode(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - msg = MAVLink_fence_status_message(breach_status, breach_count, breach_type, breach_time) - msg.pack(self) - return msg - - def fence_status_send(self, breach_status, breach_count, breach_type, breach_time): - ''' - Status of geo-fencing. Sent in extended status stream when - fencing enabled - - breach_status : 0 if currently inside fence, 1 if outside (uint8_t) - breach_count : number of fence breaches (uint16_t) - breach_type : last breach type (see FENCE_BREACH_* enum) (uint8_t) - breach_time : time of last breach in milliseconds since boot (uint32_t) - - ''' - return self.send(self.fence_status_encode(breach_status, breach_count, breach_type, breach_time)) - - def ahrs_encode(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - msg = MAVLink_ahrs_message(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw) - msg.pack(self) - return msg - - def ahrs_send(self, omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw): - ''' - Status of DCM attitude estimator - - omegaIx : X gyro drift estimate rad/s (float) - omegaIy : Y gyro drift estimate rad/s (float) - omegaIz : Z gyro drift estimate rad/s (float) - accel_weight : average accel_weight (float) - renorm_val : average renormalisation value (float) - error_rp : average error_roll_pitch value (float) - error_yaw : average error_yaw value (float) - - ''' - return self.send(self.ahrs_encode(omegaIx, omegaIy, omegaIz, accel_weight, renorm_val, error_rp, error_yaw)) - - def simstate_encode(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - msg = MAVLink_simstate_message(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro) - msg.pack(self) - return msg - - def simstate_send(self, roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro): - ''' - Status of simulation environment, if used - - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - xacc : X acceleration m/s/s (float) - yacc : Y acceleration m/s/s (float) - zacc : Z acceleration m/s/s (float) - xgyro : Angular speed around X axis rad/s (float) - ygyro : Angular speed around Y axis rad/s (float) - zgyro : Angular speed around Z axis rad/s (float) - - ''' - return self.send(self.simstate_encode(roll, pitch, yaw, xacc, yacc, zacc, xgyro, ygyro, zgyro)) - - def hwstatus_encode(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - msg = MAVLink_hwstatus_message(Vcc, I2Cerr) - msg.pack(self) - return msg - - def hwstatus_send(self, Vcc, I2Cerr): - ''' - Status of key hardware - - Vcc : board voltage (mV) (uint16_t) - I2Cerr : I2C error count (uint8_t) - - ''' - return self.send(self.hwstatus_encode(Vcc, I2Cerr)) - - def radio_encode(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - msg = MAVLink_radio_message(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed) - msg.pack(self) - return msg - - def radio_send(self, rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed): - ''' - Status generated by radio - - rssi : local signal strength (uint8_t) - remrssi : remote signal strength (uint8_t) - txbuf : how full the tx buffer is as a percentage (uint8_t) - noise : background noise level (uint8_t) - remnoise : remote background noise level (uint8_t) - rxerrors : receive errors (uint16_t) - fixed : count of error corrected packets (uint16_t) - - ''' - return self.send(self.radio_encode(rssi, remrssi, txbuf, noise, remnoise, rxerrors, fixed)) - - def heartbeat_encode(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Autopilot type / class. defined in MAV_AUTOPILOT ENUM (uint8_t) - base_mode : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t) - custom_mode : A bitfield for use for autopilot-specific flags. (uint32_t) - system_status : System status flag, see MAV_STATE ENUM (uint8_t) - mavlink_version : MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version (uint8_t) - - ''' - msg = MAVLink_heartbeat_message(type, autopilot, base_mode, custom_mode, system_status, mavlink_version) - msg.pack(self) - return msg - - def heartbeat_send(self, type, autopilot, base_mode, custom_mode, system_status, mavlink_version=3): - ''' - The heartbeat message shows that a system is present and responding. - The type of the MAV and Autopilot hardware allow the - receiving system to treat further messages from this - system appropriate (e.g. by laying out the user - interface based on the autopilot). - - type : Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) (uint8_t) - autopilot : Autopilot type / class. defined in MAV_AUTOPILOT ENUM (uint8_t) - base_mode : System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h (uint8_t) - custom_mode : A bitfield for use for autopilot-specific flags. (uint32_t) - system_status : System status flag, see MAV_STATE ENUM (uint8_t) - mavlink_version : MAVLink version, not writable by user, gets added by protocol because of magic data type: uint8_t_mavlink_version (uint8_t) - - ''' - return self.send(self.heartbeat_encode(type, autopilot, base_mode, custom_mode, system_status, mavlink_version)) - - def sys_status_encode(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - onboard_control_sensors_present : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_enabled : Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_health : Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - voltage_battery : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - current_battery : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t) - drop_rate_comm : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_comm : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_count1 : Autopilot-specific errors (uint16_t) - errors_count2 : Autopilot-specific errors (uint16_t) - errors_count3 : Autopilot-specific errors (uint16_t) - errors_count4 : Autopilot-specific errors (uint16_t) - - ''' - msg = MAVLink_sys_status_message(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4) - msg.pack(self) - return msg - - def sys_status_send(self, onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4): - ''' - The general system state. If the system is following the MAVLink - standard, the system state is mainly defined by three - orthogonal states/modes: The system mode, which is - either LOCKED (motors shut down and locked), MANUAL - (system under RC control), GUIDED (system with - autonomous position control, position setpoint - controlled manually) or AUTO (system guided by - path/waypoint planner). The NAV_MODE defined the - current flight state: LIFTOFF (often an open-loop - maneuver), LANDING, WAYPOINTS or VECTOR. This - represents the internal navigation state machine. The - system status shows wether the system is currently - active or not and if an emergency occured. During the - CRITICAL and EMERGENCY states the MAV is still - considered to be active, but should start emergency - procedures autonomously. After a failure occured it - should first move from active to critical to allow - manual intervention and then move to emergency after a - certain timeout. - - onboard_control_sensors_present : Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_enabled : Bitmask showing which onboard controllers and sensors are enabled: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - onboard_control_sensors_health : Bitmask showing which onboard controllers and sensors are operational or have an error: Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control (uint32_t) - load : Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 (uint16_t) - voltage_battery : Battery voltage, in millivolts (1 = 1 millivolt) (uint16_t) - current_battery : Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current (int16_t) - battery_remaining : Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery (int8_t) - drop_rate_comm : Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_comm : Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV) (uint16_t) - errors_count1 : Autopilot-specific errors (uint16_t) - errors_count2 : Autopilot-specific errors (uint16_t) - errors_count3 : Autopilot-specific errors (uint16_t) - errors_count4 : Autopilot-specific errors (uint16_t) - - ''' - return self.send(self.sys_status_encode(onboard_control_sensors_present, onboard_control_sensors_enabled, onboard_control_sensors_health, load, voltage_battery, current_battery, battery_remaining, drop_rate_comm, errors_comm, errors_count1, errors_count2, errors_count3, errors_count4)) - - def system_time_encode(self, time_unix_usec, time_boot_ms): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_unix_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - time_boot_ms : Timestamp of the component clock since boot time in milliseconds. (uint32_t) - - ''' - msg = MAVLink_system_time_message(time_unix_usec, time_boot_ms) - msg.pack(self) - return msg - - def system_time_send(self, time_unix_usec, time_boot_ms): - ''' - The system time is the time of the master clock, typically the - computer clock of the main onboard computer. - - time_unix_usec : Timestamp of the master clock in microseconds since UNIX epoch. (uint64_t) - time_boot_ms : Timestamp of the component clock since boot time in milliseconds. (uint32_t) - - ''' - return self.send(self.system_time_encode(time_unix_usec, time_boot_ms)) - - def ping_encode(self, time_usec, seq, target_system, target_component): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - time_usec : Unix timestamp in microseconds (uint64_t) - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - - ''' - msg = MAVLink_ping_message(time_usec, seq, target_system, target_component) - msg.pack(self) - return msg - - def ping_send(self, time_usec, seq, target_system, target_component): - ''' - A ping message either requesting or responding to a ping. This allows - to measure the system latencies, including serial - port, radio modem and UDP connections. - - time_usec : Unix timestamp in microseconds (uint64_t) - seq : PING sequence (uint32_t) - target_system : 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - target_component : 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system (uint8_t) - - ''' - return self.send(self.ping_encode(time_usec, seq, target_system, target_component)) - - def change_operator_control_encode(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - msg = MAVLink_change_operator_control_message(target_system, control_request, version, passkey) - msg.pack(self) - return msg - - def change_operator_control_send(self, target_system, control_request, version, passkey): - ''' - Request to control this MAV - - target_system : System the GCS requests control for (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - version : 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. (uint8_t) - passkey : Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" (char) - - ''' - return self.send(self.change_operator_control_encode(target_system, control_request, version, passkey)) - - def change_operator_control_ack_encode(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - msg = MAVLink_change_operator_control_ack_message(gcs_system_id, control_request, ack) - msg.pack(self) - return msg - - def change_operator_control_ack_send(self, gcs_system_id, control_request, ack): - ''' - Accept / deny control of this MAV - - gcs_system_id : ID of the GCS this message (uint8_t) - control_request : 0: request control of this MAV, 1: Release control of this MAV (uint8_t) - ack : 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control (uint8_t) - - ''' - return self.send(self.change_operator_control_ack_encode(gcs_system_id, control_request, ack)) - - def auth_key_encode(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - msg = MAVLink_auth_key_message(key) - msg.pack(self) - return msg - - def auth_key_send(self, key): - ''' - Emit an encrypted signature / key identifying this system. PLEASE - NOTE: This protocol has been kept simple, so - transmitting the key requires an encrypted channel for - true safety. - - key : key (char) - - ''' - return self.send(self.auth_key_encode(key)) - - def set_mode_encode(self, target_system, base_mode, custom_mode): - ''' - Set the system mode, as defined by enum MAV_MODE. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target_system : The system setting the mode (uint8_t) - base_mode : The new base mode (uint8_t) - custom_mode : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t) - - ''' - msg = MAVLink_set_mode_message(target_system, base_mode, custom_mode) - msg.pack(self) - return msg - - def set_mode_send(self, target_system, base_mode, custom_mode): - ''' - Set the system mode, as defined by enum MAV_MODE. There is no target - component id as the mode is by definition for the - overall aircraft, not only for one component. - - target_system : The system setting the mode (uint8_t) - base_mode : The new base mode (uint8_t) - custom_mode : The new autopilot-specific mode. This field can be ignored by an autopilot. (uint32_t) - - ''' - return self.send(self.set_mode_encode(target_system, base_mode, custom_mode)) - - def param_request_read_encode(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) (int16_t) - - ''' - msg = MAVLink_param_request_read_message(target_system, target_component, param_id, param_index) - msg.pack(self) - return msg - - def param_request_read_send(self, target_system, target_component, param_id, param_index): - ''' - Request to read the onboard parameter with the param_id string id. - Onboard parameters are stored as key[const char*] -> - value[float]. This allows to send a parameter to any - other component (such as the GCS) without the need of - previous knowledge of possible parameter names. Thus - the same GCS can store different parameters for - different autopilots. See also - http://qgroundcontrol.org/parameter_interface for a - full documentation of QGroundControl and IMU code. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_index : Parameter index. Send -1 to use the param ID field as identifier (else the param id will be ignored) (int16_t) - - ''' - return self.send(self.param_request_read_encode(target_system, target_component, param_id, param_index)) - - def param_request_list_encode(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_param_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def param_request_list_send(self, target_system, target_component): - ''' - Request all parameters of this component. After his request, all - parameters are emitted. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.param_request_list_encode(target_system, target_component)) - - def param_value_encode(self, param_id, param_value, param_type, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - msg = MAVLink_param_value_message(param_id, param_value, param_type, param_count, param_index) - msg.pack(self) - return msg - - def param_value_send(self, param_id, param_value, param_type, param_count, param_index): - ''' - Emit the value of a onboard parameter. The inclusion of param_count - and param_index in the message allows the recipient to - keep track of received parameters and allows him to - re-request missing parameters after a loss or timeout. - - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t) - param_count : Total number of onboard parameters (uint16_t) - param_index : Index of this onboard parameter (uint16_t) - - ''' - return self.send(self.param_value_encode(param_id, param_value, param_type, param_count, param_index)) - - def param_set_encode(self, target_system, target_component, param_id, param_value, param_type): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t) - - ''' - msg = MAVLink_param_set_message(target_system, target_component, param_id, param_value, param_type) - msg.pack(self) - return msg - - def param_set_send(self, target_system, target_component, param_id, param_value, param_type): - ''' - Set a parameter value TEMPORARILY to RAM. It will be reset to default - on system reboot. Send the ACTION - MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM - contents to EEPROM. IMPORTANT: The receiving component - should acknowledge the new parameter value by sending - a param_value message to all communication partners. - This will also ensure that multiple GCS all have an - up-to-date list of all parameters. If the sending GCS - did not receive a PARAM_VALUE message within its - timeout time, it should re-send the PARAM_SET message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - param_id : Onboard parameter id, terminated by NUL if the length is less than 16 human-readable chars and WITHOUT null termination (NUL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string (char) - param_value : Onboard parameter value (float) - param_type : Onboard parameter type: see MAVLINK_TYPE enum in mavlink/mavlink_types.h (uint8_t) - - ''' - return self.send(self.param_set_encode(target_system, target_component, param_id, param_value, param_type)) - - def gps_raw_int_encode(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. - See message GLOBAL_POSITION for the global position - estimate. Coordinate frame is right-handed, Z-axis up - (GPS frame). - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) above MSL (int32_t) - eph : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - epv : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - vel : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t) - cog : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - satellites_visible : Number of satellites visible. If unknown, set to 255 (uint8_t) - - ''' - msg = MAVLink_gps_raw_int_message(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible) - msg.pack(self) - return msg - - def gps_raw_int_send(self, time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible): - ''' - The global position, as returned by the Global Positioning System - (GPS). This is NOT the global position - estimate of the sytem, but rather a RAW sensor value. - See message GLOBAL_POSITION for the global position - estimate. Coordinate frame is right-handed, Z-axis up - (GPS frame). - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - fix_type : 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (uint8_t) - lat : Latitude in 1E7 degrees (int32_t) - lon : Longitude in 1E7 degrees (int32_t) - alt : Altitude in 1E3 meters (millimeters) above MSL (int32_t) - eph : GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - epv : GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 (uint16_t) - vel : GPS ground speed (m/s * 100). If unknown, set to: 65535 (uint16_t) - cog : Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - satellites_visible : Number of satellites visible. If unknown, set to 255 (uint8_t) - - ''' - return self.send(self.gps_raw_int_encode(time_usec, fix_type, lat, lon, alt, eph, epv, vel, cog, satellites_visible)) - - def gps_status_encode(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (uint8_t) - satellite_used : 0: Satellite not used, 1: used for localization (uint8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t) - satellite_snr : Signal to noise ratio of satellite (uint8_t) - - ''' - msg = MAVLink_gps_status_message(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr) - msg.pack(self) - return msg - - def gps_status_send(self, satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr): - ''' - The positioning status, as reported by GPS. This message is intended - to display status information about each satellite - visible to the receiver. See message GLOBAL_POSITION - for the global position estimate. This message can - contain information for up to 20 satellites. - - satellites_visible : Number of satellites visible (uint8_t) - satellite_prn : Global satellite ID (uint8_t) - satellite_used : 0: Satellite not used, 1: used for localization (uint8_t) - satellite_elevation : Elevation (0: right on top of receiver, 90: on the horizon) of satellite (uint8_t) - satellite_azimuth : Direction of satellite, 0: 0 deg, 255: 360 deg. (uint8_t) - satellite_snr : Signal to noise ratio of satellite (uint8_t) - - ''' - return self.send(self.gps_status_encode(satellites_visible, satellite_prn, satellite_used, satellite_elevation, satellite_azimuth, satellite_snr)) - - def scaled_imu_encode(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - msg = MAVLink_scaled_imu_message(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def scaled_imu_send(self, time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should contain the scaled values to the described - units - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - xgyro : Angular speed around X axis (millirad /sec) (int16_t) - ygyro : Angular speed around Y axis (millirad /sec) (int16_t) - zgyro : Angular speed around Z axis (millirad /sec) (int16_t) - xmag : X Magnetic field (milli tesla) (int16_t) - ymag : Y Magnetic field (milli tesla) (int16_t) - zmag : Z Magnetic field (milli tesla) (int16_t) - - ''' - return self.send(self.scaled_imu_encode(time_boot_ms, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_imu_encode(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - msg = MAVLink_raw_imu_message(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag) - msg.pack(self) - return msg - - def raw_imu_send(self, time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag): - ''' - The RAW IMU readings for the usual 9DOF sensor setup. This message - should always contain the true raw values without any - scaling to allow data capture and system debugging. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - xacc : X acceleration (raw) (int16_t) - yacc : Y acceleration (raw) (int16_t) - zacc : Z acceleration (raw) (int16_t) - xgyro : Angular speed around X axis (raw) (int16_t) - ygyro : Angular speed around Y axis (raw) (int16_t) - zgyro : Angular speed around Z axis (raw) (int16_t) - xmag : X Magnetic field (raw) (int16_t) - ymag : Y Magnetic field (raw) (int16_t) - zmag : Z Magnetic field (raw) (int16_t) - - ''' - return self.send(self.raw_imu_encode(time_usec, xacc, yacc, zacc, xgyro, ygyro, zgyro, xmag, ymag, zmag)) - - def raw_pressure_encode(self, time_usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - msg = MAVLink_raw_pressure_message(time_usec, press_abs, press_diff1, press_diff2, temperature) - msg.pack(self) - return msg - - def raw_pressure_send(self, time_usec, press_abs, press_diff1, press_diff2, temperature): - ''' - The RAW pressure readings for the typical setup of one absolute - pressure and one differential pressure sensor. The - sensor values should be the raw, UNSCALED ADC values. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - press_abs : Absolute pressure (raw) (int16_t) - press_diff1 : Differential pressure 1 (raw) (int16_t) - press_diff2 : Differential pressure 2 (raw) (int16_t) - temperature : Raw Temperature measurement (raw) (int16_t) - - ''' - return self.send(self.raw_pressure_encode(time_usec, press_abs, press_diff1, press_diff2, temperature)) - - def scaled_pressure_encode(self, time_boot_ms, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - time_boot_ms : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - msg = MAVLink_scaled_pressure_message(time_boot_ms, press_abs, press_diff, temperature) - msg.pack(self) - return msg - - def scaled_pressure_send(self, time_boot_ms, press_abs, press_diff, temperature): - ''' - The pressure readings for the typical setup of one absolute and - differential pressure sensor. The units are as - specified in each field. - - time_boot_ms : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint32_t) - press_abs : Absolute pressure (hectopascal) (float) - press_diff : Differential pressure 1 (hectopascal) (float) - temperature : Temperature measurement (0.01 degrees celsius) (int16_t) - - ''' - return self.send(self.scaled_pressure_encode(time_boot_ms, press_abs, press_diff, temperature)) - - def attitude_encode(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_message(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_send(self, time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right). - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_encode(time_boot_ms, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed)) - - def attitude_quaternion_encode(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right), expressed as quaternion. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - q1 : Quaternion component 1 (float) - q2 : Quaternion component 2 (float) - q3 : Quaternion component 3 (float) - q4 : Quaternion component 4 (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - msg = MAVLink_attitude_quaternion_message(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed) - msg.pack(self) - return msg - - def attitude_quaternion_send(self, time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed): - ''' - The attitude in the aeronautical frame (right-handed, Z-down, X-front, - Y-right), expressed as quaternion. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - q1 : Quaternion component 1 (float) - q2 : Quaternion component 2 (float) - q3 : Quaternion component 3 (float) - q4 : Quaternion component 4 (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - - ''' - return self.send(self.attitude_quaternion_encode(time_boot_ms, q1, q2, q3, q4, rollspeed, pitchspeed, yawspeed)) - - def local_position_ned_encode(self, time_boot_ms, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - msg = MAVLink_local_position_ned_message(time_boot_ms, x, y, z, vx, vy, vz) - msg.pack(self) - return msg - - def local_position_ned_send(self, time_boot_ms, x, y, z, vx, vy, vz): - ''' - The filtered local position (e.g. fused computer vision and - accelerometers). Coordinate frame is right-handed, - Z-axis down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - vx : X Speed (float) - vy : Y Speed (float) - vz : Z Speed (float) - - ''' - return self.send(self.local_position_ned_encode(time_boot_ms, x, y, z, vx, vy, vz)) - - def global_position_int_encode(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up). It - is designed as scaled integer message since the - resolution of float is not sufficient. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t) - relative_alt : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - hdg : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - - ''' - msg = MAVLink_global_position_int_message(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg) - msg.pack(self) - return msg - - def global_position_int_send(self, time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg): - ''' - The filtered global position (e.g. fused GPS and accelerometers). The - position is in GPS-frame (right-handed, Z-up). It - is designed as scaled integer message since the - resolution of float is not sufficient. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters), above MSL (int32_t) - relative_alt : Altitude above ground in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - hdg : Compass heading in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 (uint16_t) - - ''' - return self.send(self.global_position_int_encode(time_boot_ms, lat, lon, alt, relative_alt, vx, vy, vz, hdg)) - - def rc_channels_scaled_encode(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_scaled_message(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi) - msg.pack(self) - return msg - - def rc_channels_scaled_send(self, time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi): - ''' - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, - (100%) 10000 - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_scaled : RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan2_scaled : RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan3_scaled : RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan4_scaled : RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan5_scaled : RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan6_scaled : RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan7_scaled : RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - chan8_scaled : RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 (int16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_scaled_encode(time_boot_ms, port, chan1_scaled, chan2_scaled, chan3_scaled, chan4_scaled, chan5_scaled, chan6_scaled, chan7_scaled, chan8_scaled, rssi)) - - def rc_channels_raw_encode(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_rc_channels_raw_message(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi) - msg.pack(self) - return msg - - def rc_channels_raw_send(self, time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi): - ''' - The RAW values of the RC channels received. The standard PPM - modulation is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. Individual receivers/transmitters - might violate this specification. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.rc_channels_raw_encode(time_boot_ms, port, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, rssi)) - - def servo_output_raw_encode(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - time_usec : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_servo_output_raw_message(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw) - msg.pack(self) - return msg - - def servo_output_raw_send(self, time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw): - ''' - The RAW values of the servo outputs (for RC input from the remote, use - the RC_CHANNELS messages). The standard PPM modulation - is as follows: 1000 microseconds: 0%, 2000 - microseconds: 100%. - - time_usec : Timestamp (since UNIX epoch or microseconds since system boot) (uint32_t) - port : Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos. (uint8_t) - servo1_raw : Servo output 1 value, in microseconds (uint16_t) - servo2_raw : Servo output 2 value, in microseconds (uint16_t) - servo3_raw : Servo output 3 value, in microseconds (uint16_t) - servo4_raw : Servo output 4 value, in microseconds (uint16_t) - servo5_raw : Servo output 5 value, in microseconds (uint16_t) - servo6_raw : Servo output 6 value, in microseconds (uint16_t) - servo7_raw : Servo output 7 value, in microseconds (uint16_t) - servo8_raw : Servo output 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.servo_output_raw_encode(time_usec, port, servo1_raw, servo2_raw, servo3_raw, servo4_raw, servo5_raw, servo6_raw, servo7_raw, servo8_raw)) - - def mission_request_partial_list_encode(self, target_system, target_component, start_index, end_index): - ''' - Request the overall list of MISSIONs from the system/component. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default (int16_t) - end_index : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t) - - ''' - msg = MAVLink_mission_request_partial_list_message(target_system, target_component, start_index, end_index) - msg.pack(self) - return msg - - def mission_request_partial_list_send(self, target_system, target_component, start_index, end_index): - ''' - Request the overall list of MISSIONs from the system/component. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default (int16_t) - end_index : End index, -1 by default (-1: send list to end). Else a valid index of the list (int16_t) - - ''' - return self.send(self.mission_request_partial_list_encode(target_system, target_component, start_index, end_index)) - - def mission_write_partial_list_encode(self, target_system, target_component, start_index, end_index): - ''' - This message is sent to the MAV to write a partial list. If start - index == end index, only one item will be transmitted - / updated. If the start index is NOT 0 and above the - current list size, this request should be REJECTED! - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t) - end_index : End index, equal or greater than start index. (int16_t) - - ''' - msg = MAVLink_mission_write_partial_list_message(target_system, target_component, start_index, end_index) - msg.pack(self) - return msg - - def mission_write_partial_list_send(self, target_system, target_component, start_index, end_index): - ''' - This message is sent to the MAV to write a partial list. If start - index == end index, only one item will be transmitted - / updated. If the start index is NOT 0 and above the - current list size, this request should be REJECTED! - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - start_index : Start index, 0 by default and smaller / equal to the largest index of the current onboard list. (int16_t) - end_index : End index, equal or greater than start index. (int16_t) - - ''' - return self.send(self.mission_write_partial_list_encode(target_system, target_component, start_index, end_index)) - - def mission_item_encode(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a mission item. This message is emitted to announce - the presence of a mission item and to set a mission - item on the system. The mission item can be either in - x, y, z meters (type: LOCAL) or x:lat, y:lon, - z:altitude. Local frame is Z-down, right handed (NED), - global frame is Z-up, right handed (ENU). See also - http://qgroundcontrol.org/mavlink/waypoint_protocol. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - msg = MAVLink_mission_item_message(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z) - msg.pack(self) - return msg - - def mission_item_send(self, target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z): - ''' - Message encoding a mission item. This message is emitted to announce - the presence of a mission item and to set a mission - item on the system. The mission item can be either in - x, y, z meters (type: LOCAL) or x:lat, y:lon, - z:altitude. Local frame is Z-down, right handed (NED), - global frame is Z-up, right handed (ENU). See also - http://qgroundcontrol.org/mavlink/waypoint_protocol. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - frame : The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h (uint8_t) - command : The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs (uint16_t) - current : false:0, true:1 (uint8_t) - autocontinue : autocontinue to next wp (uint8_t) - param1 : PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters (float) - param2 : PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds (float) - param3 : PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. (float) - param4 : PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH (float) - x : PARAM5 / local: x position, global: latitude (float) - y : PARAM6 / y position: global: longitude (float) - z : PARAM7 / z position: global: altitude (float) - - ''' - return self.send(self.mission_item_encode(target_system, target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z)) - - def mission_request_encode(self, target_system, target_component, seq): - ''' - Request the information of the mission item with the sequence number - seq. The response of the system to this message should - be a MISSION_ITEM message. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_request_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def mission_request_send(self, target_system, target_component, seq): - ''' - Request the information of the mission item with the sequence number - seq. The response of the system to this message should - be a MISSION_ITEM message. - http://qgroundcontrol.org/mavlink/waypoint_protocol - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_request_encode(target_system, target_component, seq)) - - def mission_set_current_encode(self, target_system, target_component, seq): - ''' - Set the mission item with sequence number seq as current item. This - means that the MAV will continue to this mission item - on the shortest path (not following the mission items - in-between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_set_current_message(target_system, target_component, seq) - msg.pack(self) - return msg - - def mission_set_current_send(self, target_system, target_component, seq): - ''' - Set the mission item with sequence number seq as current item. This - means that the MAV will continue to this mission item - on the shortest path (not following the mission items - in-between). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_set_current_encode(target_system, target_component, seq)) - - def mission_current_encode(self, seq): - ''' - Message that announces the sequence number of the current active - mission item. The MAV will fly towards this mission - item. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_current_message(seq) - msg.pack(self) - return msg - - def mission_current_send(self, seq): - ''' - Message that announces the sequence number of the current active - mission item. The MAV will fly towards this mission - item. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_current_encode(seq)) - - def mission_request_list_encode(self, target_system, target_component): - ''' - Request the overall list of mission items from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_mission_request_list_message(target_system, target_component) - msg.pack(self) - return msg - - def mission_request_list_send(self, target_system, target_component): - ''' - Request the overall list of mission items from the system/component. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.mission_request_list_encode(target_system, target_component)) - - def mission_count_encode(self, target_system, target_component, count): - ''' - This message is emitted as response to MISSION_REQUEST_LIST by the MAV - and to initiate a write transaction. The GCS can then - request the individual mission item based on the - knowledge of the total number of MISSIONs. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of mission items in the sequence (uint16_t) - - ''' - msg = MAVLink_mission_count_message(target_system, target_component, count) - msg.pack(self) - return msg - - def mission_count_send(self, target_system, target_component, count): - ''' - This message is emitted as response to MISSION_REQUEST_LIST by the MAV - and to initiate a write transaction. The GCS can then - request the individual mission item based on the - knowledge of the total number of MISSIONs. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - count : Number of mission items in the sequence (uint16_t) - - ''' - return self.send(self.mission_count_encode(target_system, target_component, count)) - - def mission_clear_all_encode(self, target_system, target_component): - ''' - Delete all mission items at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - msg = MAVLink_mission_clear_all_message(target_system, target_component) - msg.pack(self) - return msg - - def mission_clear_all_send(self, target_system, target_component): - ''' - Delete all mission items at once. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - - ''' - return self.send(self.mission_clear_all_encode(target_system, target_component)) - - def mission_item_reached_encode(self, seq): - ''' - A certain mission item has been reached. The system will either hold - this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - MISSION. - - seq : Sequence (uint16_t) - - ''' - msg = MAVLink_mission_item_reached_message(seq) - msg.pack(self) - return msg - - def mission_item_reached_send(self, seq): - ''' - A certain mission item has been reached. The system will either hold - this position (or circle on the orbit) or (if the - autocontinue on the WP was set) continue to the next - MISSION. - - seq : Sequence (uint16_t) - - ''' - return self.send(self.mission_item_reached_encode(seq)) - - def mission_ack_encode(self, target_system, target_component, type): - ''' - Ack message during MISSION handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : See MAV_MISSION_RESULT enum (uint8_t) - - ''' - msg = MAVLink_mission_ack_message(target_system, target_component, type) - msg.pack(self) - return msg - - def mission_ack_send(self, target_system, target_component, type): - ''' - Ack message during MISSION handling. The type field states if this - message is a positive ack (type=0) or if an error - happened (type=non-zero). - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - type : See MAV_MISSION_RESULT enum (uint8_t) - - ''' - return self.send(self.mission_ack_encode(target_system, target_component, type)) - - def set_gps_global_origin_encode(self, target_system, latitude, longitude, altitude): - ''' - As local MISSIONs exist, the global MISSION reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - msg = MAVLink_set_gps_global_origin_message(target_system, latitude, longitude, altitude) - msg.pack(self) - return msg - - def set_gps_global_origin_send(self, target_system, latitude, longitude, altitude): - ''' - As local MISSIONs exist, the global MISSION reference allows to - transform between the local coordinate frame and the - global (GPS) coordinate frame. This can be necessary - when e.g. in- and outdoor settings are connected and - the MAV should move from in- to outdoor. - - target_system : System ID (uint8_t) - latitude : global position * 1E7 (int32_t) - longitude : global position * 1E7 (int32_t) - altitude : global position * 1000 (int32_t) - - ''' - return self.send(self.set_gps_global_origin_encode(target_system, latitude, longitude, altitude)) - - def gps_global_origin_encode(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - msg = MAVLink_gps_global_origin_message(latitude, longitude, altitude) - msg.pack(self) - return msg - - def gps_global_origin_send(self, latitude, longitude, altitude): - ''' - Once the MAV sets a new GPS-Local correspondence, this message - announces the origin (0,0,0) position - - latitude : Latitude (WGS84), expressed as * 1E7 (int32_t) - longitude : Longitude (WGS84), expressed as * 1E7 (int32_t) - altitude : Altitude(WGS84), expressed as * 1000 (int32_t) - - ''' - return self.send(self.gps_global_origin_encode(latitude, longitude, altitude)) - - def set_local_position_setpoint_encode(self, target_system, target_component, coordinate_frame, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/MISSION planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_set_local_position_setpoint_message(target_system, target_component, coordinate_frame, x, y, z, yaw) - msg.pack(self) - return msg - - def set_local_position_setpoint_send(self, target_system, target_component, coordinate_frame, x, y, z, yaw): - ''' - Set the setpoint for a local position controller. This is the position - in local coordinates the MAV should fly to. This - message is sent by the path/MISSION planner to the - onboard position controller. As some MAVs have a - degree of freedom in yaw (e.g. all - helicopters/quadrotors), the desired yaw angle is part - of the message. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.set_local_position_setpoint_encode(target_system, target_component, coordinate_frame, x, y, z, yaw)) - - def local_position_setpoint_encode(self, coordinate_frame, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - msg = MAVLink_local_position_setpoint_message(coordinate_frame, x, y, z, yaw) - msg.pack(self) - return msg - - def local_position_setpoint_send(self, coordinate_frame, x, y, z, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_LOCAL_NED or MAV_FRAME_LOCAL_ENU (uint8_t) - x : x position (float) - y : y position (float) - z : z position (float) - yaw : Desired yaw angle (float) - - ''' - return self.send(self.local_position_setpoint_encode(coordinate_frame, x, y, z, yaw)) - - def global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - msg = MAVLink_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) - msg.pack(self) - return msg - - def global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Transmit the current local setpoint of the controller to other MAVs - (collision avoidance) and to the GCS. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - return self.send(self.global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - - def set_global_position_setpoint_int_encode(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Set the current global position setpoint. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - msg = MAVLink_set_global_position_setpoint_int_message(coordinate_frame, latitude, longitude, altitude, yaw) - msg.pack(self) - return msg - - def set_global_position_setpoint_int_send(self, coordinate_frame, latitude, longitude, altitude, yaw): - ''' - Set the current global position setpoint. - - coordinate_frame : Coordinate frame - valid values are only MAV_FRAME_GLOBAL or MAV_FRAME_GLOBAL_RELATIVE_ALT (uint8_t) - latitude : WGS84 Latitude position in degrees * 1E7 (int32_t) - longitude : WGS84 Longitude position in degrees * 1E7 (int32_t) - altitude : WGS84 Altitude in meters * 1000 (positive for up) (int32_t) - yaw : Desired yaw angle in degrees * 100 (int16_t) - - ''' - return self.send(self.set_global_position_setpoint_int_encode(coordinate_frame, latitude, longitude, altitude, yaw)) - - def safety_set_allowed_area_encode(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/MISSIONs to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_set_allowed_area_message(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_set_allowed_area_send(self, target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Set a safety zone (volume), which is defined by two corners of a cube. - This message can be used to tell the MAV which - setpoints/MISSIONs to accept and which to reject. - Safety areas are often enforced by national or - competition regulations. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_set_allowed_area_encode(target_system, target_component, frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def safety_allowed_area_encode(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - msg = MAVLink_safety_allowed_area_message(frame, p1x, p1y, p1z, p2x, p2y, p2z) - msg.pack(self) - return msg - - def safety_allowed_area_send(self, frame, p1x, p1y, p1z, p2x, p2y, p2z): - ''' - Read out the safety zone the MAV currently assumes. - - frame : Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. (uint8_t) - p1x : x position 1 / Latitude 1 (float) - p1y : y position 1 / Longitude 1 (float) - p1z : z position 1 / Altitude 1 (float) - p2x : x position 2 / Latitude 2 (float) - p2y : y position 2 / Longitude 2 (float) - p2z : z position 2 / Altitude 2 (float) - - ''' - return self.send(self.safety_allowed_area_encode(frame, p1x, p1y, p1z, p2x, p2y, p2z)) - - def set_roll_pitch_yaw_thrust_encode(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_thrust_message(target_system, target_component, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_thrust_send(self, target_system, target_component, roll, pitch, yaw, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_thrust_encode(target_system, target_component, roll, pitch, yaw, thrust)) - - def set_roll_pitch_yaw_speed_thrust_encode(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_set_roll_pitch_yaw_speed_thrust_message(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def set_roll_pitch_yaw_speed_thrust_send(self, target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Set roll, pitch and yaw. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.set_roll_pitch_yaw_speed_thrust_encode(target_system, target_component, roll_speed, pitch_speed, yaw_speed, thrust)) - - def roll_pitch_yaw_thrust_setpoint_encode(self, time_boot_ms, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_thrust_setpoint_message(time_boot_ms, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_thrust_setpoint_send(self, time_boot_ms, roll, pitch, yaw, thrust): - ''' - Setpoint in roll, pitch, yaw currently active on the system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll : Desired roll angle in radians (float) - pitch : Desired pitch angle in radians (float) - yaw : Desired yaw angle in radians (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_thrust_setpoint_encode(time_boot_ms, roll, pitch, yaw, thrust)) - - def roll_pitch_yaw_speed_thrust_setpoint_encode(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - msg = MAVLink_roll_pitch_yaw_speed_thrust_setpoint_message(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust) - msg.pack(self) - return msg - - def roll_pitch_yaw_speed_thrust_setpoint_send(self, time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust): - ''' - Setpoint in rollspeed, pitchspeed, yawspeed currently active on the - system. - - time_boot_ms : Timestamp in milliseconds since system boot (uint32_t) - roll_speed : Desired roll angular speed in rad/s (float) - pitch_speed : Desired pitch angular speed in rad/s (float) - yaw_speed : Desired yaw angular speed in rad/s (float) - thrust : Collective thrust, normalized to 0 .. 1 (float) - - ''' - return self.send(self.roll_pitch_yaw_speed_thrust_setpoint_encode(time_boot_ms, roll_speed, pitch_speed, yaw_speed, thrust)) - - def set_quad_motors_setpoint_encode(self, target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw): - ''' - Setpoint in the four motor speeds - - target_system : System ID of the system that should set these motor commands (uint8_t) - motor_front_nw : Front motor in + configuration, front left motor in x configuration (uint16_t) - motor_right_ne : Right motor in + configuration, front right motor in x configuration (uint16_t) - motor_back_se : Back motor in + configuration, back right motor in x configuration (uint16_t) - motor_left_sw : Left motor in + configuration, back left motor in x configuration (uint16_t) - - ''' - msg = MAVLink_set_quad_motors_setpoint_message(target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw) - msg.pack(self) - return msg - - def set_quad_motors_setpoint_send(self, target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw): - ''' - Setpoint in the four motor speeds - - target_system : System ID of the system that should set these motor commands (uint8_t) - motor_front_nw : Front motor in + configuration, front left motor in x configuration (uint16_t) - motor_right_ne : Right motor in + configuration, front right motor in x configuration (uint16_t) - motor_back_se : Back motor in + configuration, back right motor in x configuration (uint16_t) - motor_left_sw : Left motor in + configuration, back left motor in x configuration (uint16_t) - - ''' - return self.send(self.set_quad_motors_setpoint_encode(target_system, motor_front_nw, motor_right_ne, motor_back_se, motor_left_sw)) - - def set_quad_swarm_roll_pitch_yaw_thrust_encode(self, group, mode, roll, pitch, yaw, thrust): - ''' - Setpoint for up to four quadrotors in a group / wing - - group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t) - mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t) - roll : Desired roll angle in radians +-PI (+-32767) (int16_t) - pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t) - thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t) - - ''' - msg = MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(group, mode, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_quad_swarm_roll_pitch_yaw_thrust_send(self, group, mode, roll, pitch, yaw, thrust): - ''' - Setpoint for up to four quadrotors in a group / wing - - group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t) - mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t) - roll : Desired roll angle in radians +-PI (+-32767) (int16_t) - pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t) - thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t) - - ''' - return self.send(self.set_quad_swarm_roll_pitch_yaw_thrust_encode(group, mode, roll, pitch, yaw, thrust)) - - def nav_controller_output_encode(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters. - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current MISSION/target in degrees (int16_t) - wp_dist : Distance to active MISSION in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - msg = MAVLink_nav_controller_output_message(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error) - msg.pack(self) - return msg - - def nav_controller_output_send(self, nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error): - ''' - Outputs of the APM navigation controller. The primary use of this - message is to check the response and signs of the - controller before actual flight and to assist with - tuning controller parameters. - - nav_roll : Current desired roll in degrees (float) - nav_pitch : Current desired pitch in degrees (float) - nav_bearing : Current desired heading in degrees (int16_t) - target_bearing : Bearing to current MISSION/target in degrees (int16_t) - wp_dist : Distance to active MISSION in meters (uint16_t) - alt_error : Current altitude error in meters (float) - aspd_error : Current airspeed error in meters/second (float) - xtrack_error : Current crosstrack error on x-y plane in meters (float) - - ''' - return self.send(self.nav_controller_output_encode(nav_roll, nav_pitch, nav_bearing, target_bearing, wp_dist, alt_error, aspd_error, xtrack_error)) - - def set_quad_swarm_led_roll_pitch_yaw_thrust_encode(self, group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust): - ''' - Setpoint for up to four quadrotors in a group / wing - - group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t) - mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t) - led_red : RGB red channel (0-255) (uint8_t) - led_blue : RGB green channel (0-255) (uint8_t) - led_green : RGB blue channel (0-255) (uint8_t) - roll : Desired roll angle in radians +-PI (+-32767) (int16_t) - pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t) - thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t) - - ''' - msg = MAVLink_set_quad_swarm_led_roll_pitch_yaw_thrust_message(group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust) - msg.pack(self) - return msg - - def set_quad_swarm_led_roll_pitch_yaw_thrust_send(self, group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust): - ''' - Setpoint for up to four quadrotors in a group / wing - - group : ID of the quadrotor group (0 - 255, up to 256 groups supported) (uint8_t) - mode : ID of the flight mode (0 - 255, up to 256 modes supported) (uint8_t) - led_red : RGB red channel (0-255) (uint8_t) - led_blue : RGB green channel (0-255) (uint8_t) - led_green : RGB blue channel (0-255) (uint8_t) - roll : Desired roll angle in radians +-PI (+-32767) (int16_t) - pitch : Desired pitch angle in radians +-PI (+-32767) (int16_t) - yaw : Desired yaw angle in radians, scaled to int16 +-PI (+-32767) (int16_t) - thrust : Collective thrust, scaled to uint16 (0..65535) (uint16_t) - - ''' - return self.send(self.set_quad_swarm_led_roll_pitch_yaw_thrust_encode(group, mode, led_red, led_blue, led_green, roll, pitch, yaw, thrust)) - - def state_correction_encode(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - msg = MAVLink_state_correction_message(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr) - msg.pack(self) - return msg - - def state_correction_send(self, xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr): - ''' - Corrects the systems state by adding an error correction term to the - position and velocity, and by rotating the attitude by - a correction angle. - - xErr : x position error (float) - yErr : y position error (float) - zErr : z position error (float) - rollErr : roll error (radians) (float) - pitchErr : pitch error (radians) (float) - yawErr : yaw error (radians) (float) - vxErr : x velocity (float) - vyErr : y velocity (float) - vzErr : z velocity (float) - - ''' - return self.send(self.state_correction_encode(xErr, yErr, zErr, rollErr, pitchErr, yawErr, vxErr, vyErr, vzErr)) - - def request_data_stream_encode(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested data stream (uint8_t) - req_message_rate : The requested interval between two messages of this type (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - msg = MAVLink_request_data_stream_message(target_system, target_component, req_stream_id, req_message_rate, start_stop) - msg.pack(self) - return msg - - def request_data_stream_send(self, target_system, target_component, req_stream_id, req_message_rate, start_stop): - ''' - - - target_system : The target requested to send the message stream. (uint8_t) - target_component : The target requested to send the message stream. (uint8_t) - req_stream_id : The ID of the requested data stream (uint8_t) - req_message_rate : The requested interval between two messages of this type (uint16_t) - start_stop : 1 to start sending, 0 to stop sending. (uint8_t) - - ''' - return self.send(self.request_data_stream_encode(target_system, target_component, req_stream_id, req_message_rate, start_stop)) - - def data_stream_encode(self, stream_id, message_rate, on_off): - ''' - - - stream_id : The ID of the requested data stream (uint8_t) - message_rate : The requested interval between two messages of this type (uint16_t) - on_off : 1 stream is enabled, 0 stream is stopped. (uint8_t) - - ''' - msg = MAVLink_data_stream_message(stream_id, message_rate, on_off) - msg.pack(self) - return msg - - def data_stream_send(self, stream_id, message_rate, on_off): - ''' - - - stream_id : The ID of the requested data stream (uint8_t) - message_rate : The requested interval between two messages of this type (uint16_t) - on_off : 1 stream is enabled, 0 stream is stopped. (uint8_t) - - ''' - return self.send(self.data_stream_encode(stream_id, message_rate, on_off)) - - def manual_control_encode(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - msg = MAVLink_manual_control_message(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual) - msg.pack(self) - return msg - - def manual_control_send(self, target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual): - ''' - - - target : The system to be controlled (uint8_t) - roll : roll (float) - pitch : pitch (float) - yaw : yaw (float) - thrust : thrust (float) - roll_manual : roll control enabled auto:0, manual:1 (uint8_t) - pitch_manual : pitch auto:0, manual:1 (uint8_t) - yaw_manual : yaw auto:0, manual:1 (uint8_t) - thrust_manual : thrust auto:0, manual:1 (uint8_t) - - ''' - return self.send(self.manual_control_encode(target, roll, pitch, yaw, thrust, roll_manual, pitch_manual, yaw_manual, thrust_manual)) - - def rc_channels_override_encode(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - msg = MAVLink_rc_channels_override_message(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw) - msg.pack(self) - return msg - - def rc_channels_override_send(self, target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw): - ''' - The RAW values of the RC channels sent to the MAV to override info - received from the RC radio. A value of -1 means no - change to that channel. A value of 0 means control of - that channel should be released back to the RC radio. - The standard PPM modulation is as follows: 1000 - microseconds: 0%, 2000 microseconds: 100%. Individual - receivers/transmitters might violate this - specification. - - target_system : System ID (uint8_t) - target_component : Component ID (uint8_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - - ''' - return self.send(self.rc_channels_override_encode(target_system, target_component, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw)) - - def vfr_hud_encode(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - msg = MAVLink_vfr_hud_message(airspeed, groundspeed, heading, throttle, alt, climb) - msg.pack(self) - return msg - - def vfr_hud_send(self, airspeed, groundspeed, heading, throttle, alt, climb): - ''' - Metrics typically displayed on a HUD for fixed wing aircraft - - airspeed : Current airspeed in m/s (float) - groundspeed : Current ground speed in m/s (float) - heading : Current heading in degrees, in compass units (0..360, 0=north) (int16_t) - throttle : Current throttle setting in integer percent, 0 to 100 (uint16_t) - alt : Current altitude (MSL), in meters (float) - climb : Current climb rate in meters/second (float) - - ''' - return self.send(self.vfr_hud_encode(airspeed, groundspeed, heading, throttle, alt, climb)) - - def command_long_encode(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - param5 : Parameter 5, as defined by MAV_CMD enum. (float) - param6 : Parameter 6, as defined by MAV_CMD enum. (float) - param7 : Parameter 7, as defined by MAV_CMD enum. (float) - - ''' - msg = MAVLink_command_long_message(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7) - msg.pack(self) - return msg - - def command_long_send(self, target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7): - ''' - Send a command with up to four parameters to the MAV - - target_system : System which should execute the command (uint8_t) - target_component : Component which should execute the command, 0 for all components (uint8_t) - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - confirmation : 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) (uint8_t) - param1 : Parameter 1, as defined by MAV_CMD enum. (float) - param2 : Parameter 2, as defined by MAV_CMD enum. (float) - param3 : Parameter 3, as defined by MAV_CMD enum. (float) - param4 : Parameter 4, as defined by MAV_CMD enum. (float) - param5 : Parameter 5, as defined by MAV_CMD enum. (float) - param6 : Parameter 6, as defined by MAV_CMD enum. (float) - param7 : Parameter 7, as defined by MAV_CMD enum. (float) - - ''' - return self.send(self.command_long_encode(target_system, target_component, command, confirmation, param1, param2, param3, param4, param5, param6, param7)) - - def command_ack_encode(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed. - - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - result : See MAV_RESULT enum (uint8_t) - - ''' - msg = MAVLink_command_ack_message(command, result) - msg.pack(self) - return msg - - def command_ack_send(self, command, result): - ''' - Report status of a command. Includes feedback wether the command was - executed. - - command : Command ID, as defined by MAV_CMD enum. (uint16_t) - result : See MAV_RESULT enum (uint8_t) - - ''' - return self.send(self.command_ack_encode(command, result)) - - def local_position_ned_system_global_offset_encode(self, time_boot_ms, x, y, z, roll, pitch, yaw): - ''' - The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages - of MAV X and the global coordinate frame in NED - coordinates. Coordinate frame is right-handed, Z-axis - down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - roll : Roll (float) - pitch : Pitch (float) - yaw : Yaw (float) - - ''' - msg = MAVLink_local_position_ned_system_global_offset_message(time_boot_ms, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def local_position_ned_system_global_offset_send(self, time_boot_ms, x, y, z, roll, pitch, yaw): - ''' - The offset in X, Y, Z and yaw between the LOCAL_POSITION_NED messages - of MAV X and the global coordinate frame in NED - coordinates. Coordinate frame is right-handed, Z-axis - down (aeronautical frame, NED / north-east-down - convention) - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - x : X Position (float) - y : Y Position (float) - z : Z Position (float) - roll : Roll (float) - pitch : Pitch (float) - yaw : Yaw (float) - - ''' - return self.send(self.local_position_ned_system_global_offset_encode(time_boot_ms, x, y, z, roll, pitch, yaw)) - - def hil_state_encode(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - Sent from simulation to autopilot. This packet is useful for high - throughput applications such as hardware in the loop - simulations. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - msg = MAVLink_hil_state_message(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc) - msg.pack(self) - return msg - - def hil_state_send(self, time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc): - ''' - Sent from simulation to autopilot. This packet is useful for high - throughput applications such as hardware in the loop - simulations. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll : Roll angle (rad) (float) - pitch : Pitch angle (rad) (float) - yaw : Yaw angle (rad) (float) - rollspeed : Roll angular speed (rad/s) (float) - pitchspeed : Pitch angular speed (rad/s) (float) - yawspeed : Yaw angular speed (rad/s) (float) - lat : Latitude, expressed as * 1E7 (int32_t) - lon : Longitude, expressed as * 1E7 (int32_t) - alt : Altitude in meters, expressed as * 1000 (millimeters) (int32_t) - vx : Ground X Speed (Latitude), expressed as m/s * 100 (int16_t) - vy : Ground Y Speed (Longitude), expressed as m/s * 100 (int16_t) - vz : Ground Z Speed (Altitude), expressed as m/s * 100 (int16_t) - xacc : X acceleration (mg) (int16_t) - yacc : Y acceleration (mg) (int16_t) - zacc : Z acceleration (mg) (int16_t) - - ''' - return self.send(self.hil_state_encode(time_usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc)) - - def hil_controls_encode(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): - ''' - Sent from autopilot to simulation. Hardware in the loop control - outputs - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -1 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - aux1 : Aux 1, -1 .. 1 (float) - aux2 : Aux 2, -1 .. 1 (float) - aux3 : Aux 3, -1 .. 1 (float) - aux4 : Aux 4, -1 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - msg = MAVLink_hil_controls_message(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode) - msg.pack(self) - return msg - - def hil_controls_send(self, time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode): - ''' - Sent from autopilot to simulation. Hardware in the loop control - outputs - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - roll_ailerons : Control output -1 .. 1 (float) - pitch_elevator : Control output -1 .. 1 (float) - yaw_rudder : Control output -1 .. 1 (float) - throttle : Throttle 0 .. 1 (float) - aux1 : Aux 1, -1 .. 1 (float) - aux2 : Aux 2, -1 .. 1 (float) - aux3 : Aux 3, -1 .. 1 (float) - aux4 : Aux 4, -1 .. 1 (float) - mode : System mode (MAV_MODE) (uint8_t) - nav_mode : Navigation mode (MAV_NAV_MODE) (uint8_t) - - ''' - return self.send(self.hil_controls_encode(time_usec, roll_ailerons, pitch_elevator, yaw_rudder, throttle, aux1, aux2, aux3, aux4, mode, nav_mode)) - - def hil_rc_inputs_raw_encode(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): - ''' - Sent from simulation to autopilot. The RAW values of the RC channels - received. The standard PPM modulation is as follows: - 1000 microseconds: 0%, 2000 microseconds: 100%. - Individual receivers/transmitters might violate this - specification. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - chan9_raw : RC channel 9 value, in microseconds (uint16_t) - chan10_raw : RC channel 10 value, in microseconds (uint16_t) - chan11_raw : RC channel 11 value, in microseconds (uint16_t) - chan12_raw : RC channel 12 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - msg = MAVLink_hil_rc_inputs_raw_message(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi) - msg.pack(self) - return msg - - def hil_rc_inputs_raw_send(self, time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi): - ''' - Sent from simulation to autopilot. The RAW values of the RC channels - received. The standard PPM modulation is as follows: - 1000 microseconds: 0%, 2000 microseconds: 100%. - Individual receivers/transmitters might violate this - specification. - - time_usec : Timestamp (microseconds since UNIX epoch or microseconds since system boot) (uint64_t) - chan1_raw : RC channel 1 value, in microseconds (uint16_t) - chan2_raw : RC channel 2 value, in microseconds (uint16_t) - chan3_raw : RC channel 3 value, in microseconds (uint16_t) - chan4_raw : RC channel 4 value, in microseconds (uint16_t) - chan5_raw : RC channel 5 value, in microseconds (uint16_t) - chan6_raw : RC channel 6 value, in microseconds (uint16_t) - chan7_raw : RC channel 7 value, in microseconds (uint16_t) - chan8_raw : RC channel 8 value, in microseconds (uint16_t) - chan9_raw : RC channel 9 value, in microseconds (uint16_t) - chan10_raw : RC channel 10 value, in microseconds (uint16_t) - chan11_raw : RC channel 11 value, in microseconds (uint16_t) - chan12_raw : RC channel 12 value, in microseconds (uint16_t) - rssi : Receive signal strength indicator, 0: 0%, 255: 100% (uint8_t) - - ''' - return self.send(self.hil_rc_inputs_raw_encode(time_usec, chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw, chan10_raw, chan11_raw, chan12_raw, rssi)) - - def optical_flow_encode(self, time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time_usec : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - flow_comp_m_x : Flow in meters in x-sensor direction, angular-speed compensated (float) - flow_comp_m_y : Flow in meters in y-sensor direction, angular-speed compensated (float) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters. Positive value: distance known. Negative value: Unknown distance (float) - - ''' - msg = MAVLink_optical_flow_message(time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance) - msg.pack(self) - return msg - - def optical_flow_send(self, time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance): - ''' - Optical flow from a flow sensor (e.g. optical mouse sensor) - - time_usec : Timestamp (UNIX) (uint64_t) - sensor_id : Sensor ID (uint8_t) - flow_x : Flow in pixels in x-sensor direction (int16_t) - flow_y : Flow in pixels in y-sensor direction (int16_t) - flow_comp_m_x : Flow in meters in x-sensor direction, angular-speed compensated (float) - flow_comp_m_y : Flow in meters in y-sensor direction, angular-speed compensated (float) - quality : Optical flow quality / confidence. 0: bad, 255: maximum quality (uint8_t) - ground_distance : Ground distance in meters. Positive value: distance known. Negative value: Unknown distance (float) - - ''' - return self.send(self.optical_flow_encode(time_usec, sensor_id, flow_x, flow_y, flow_comp_m_x, flow_comp_m_y, quality, ground_distance)) - - def global_vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_global_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def global_vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.global_vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def vision_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_vision_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def vision_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.vision_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def vision_speed_estimate_encode(self, usec, x, y, z): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X speed (float) - y : Global Y speed (float) - z : Global Z speed (float) - - ''' - msg = MAVLink_vision_speed_estimate_message(usec, x, y, z) - msg.pack(self) - return msg - - def vision_speed_estimate_send(self, usec, x, y, z): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X speed (float) - y : Global Y speed (float) - z : Global Z speed (float) - - ''' - return self.send(self.vision_speed_estimate_encode(usec, x, y, z)) - - def vicon_position_estimate_encode(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - msg = MAVLink_vicon_position_estimate_message(usec, x, y, z, roll, pitch, yaw) - msg.pack(self) - return msg - - def vicon_position_estimate_send(self, usec, x, y, z, roll, pitch, yaw): - ''' - - - usec : Timestamp (milliseconds) (uint64_t) - x : Global X position (float) - y : Global Y position (float) - z : Global Z position (float) - roll : Roll angle in rad (float) - pitch : Pitch angle in rad (float) - yaw : Yaw angle in rad (float) - - ''' - return self.send(self.vicon_position_estimate_encode(usec, x, y, z, roll, pitch, yaw)) - - def memory_vect_encode(self, address, ver, type, value): - ''' - Send raw controller memory. The use of this message is discouraged for - normal packets, but a quite efficient way for testing - new messages and getting experimental debug output. - - address : Starting address of the debug variables (uint16_t) - ver : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t) - type : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t) - value : Memory contents at specified address (int8_t) - - ''' - msg = MAVLink_memory_vect_message(address, ver, type, value) - msg.pack(self) - return msg - - def memory_vect_send(self, address, ver, type, value): - ''' - Send raw controller memory. The use of this message is discouraged for - normal packets, but a quite efficient way for testing - new messages and getting experimental debug output. - - address : Starting address of the debug variables (uint16_t) - ver : Version code of the type variable. 0=unknown, type ignored and assumed int16_t. 1=as below (uint8_t) - type : Type code of the memory variables. for ver = 1: 0=16 x int16_t, 1=16 x uint16_t, 2=16 x Q15, 3=16 x 1Q14 (uint8_t) - value : Memory contents at specified address (int8_t) - - ''' - return self.send(self.memory_vect_encode(address, ver, type, value)) - - def debug_vect_encode(self, name, time_usec, x, y, z): - ''' - - - name : Name (char) - time_usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - msg = MAVLink_debug_vect_message(name, time_usec, x, y, z) - msg.pack(self) - return msg - - def debug_vect_send(self, name, time_usec, x, y, z): - ''' - - - name : Name (char) - time_usec : Timestamp (uint64_t) - x : x (float) - y : y (float) - z : z (float) - - ''' - return self.send(self.debug_vect_encode(name, time_usec, x, y, z)) - - def named_value_float_encode(self, time_boot_ms, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - msg = MAVLink_named_value_float_message(time_boot_ms, name, value) - msg.pack(self) - return msg - - def named_value_float_send(self, time_boot_ms, name, value): - ''' - Send a key-value pair as float. The use of this message is discouraged - for normal packets, but a quite efficient way for - testing new messages and getting experimental debug - output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Floating point value (float) - - ''' - return self.send(self.named_value_float_encode(time_boot_ms, name, value)) - - def named_value_int_encode(self, time_boot_ms, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - msg = MAVLink_named_value_int_message(time_boot_ms, name, value) - msg.pack(self) - return msg - - def named_value_int_send(self, time_boot_ms, name, value): - ''' - Send a key-value pair as integer. The use of this message is - discouraged for normal packets, but a quite efficient - way for testing new messages and getting experimental - debug output. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - name : Name of the debug variable (char) - value : Signed integer value (int32_t) - - ''' - return self.send(self.named_value_int_encode(time_boot_ms, name, value)) - - def statustext_encode(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. (uint8_t) - text : Status text message, without null termination character (char) - - ''' - msg = MAVLink_statustext_message(severity, text) - msg.pack(self) - return msg - - def statustext_send(self, severity, text): - ''' - Status text message. These messages are printed in yellow in the COMM - console of QGroundControl. WARNING: They consume quite - some bandwidth, so use only for important status and - error messages. If implemented wisely, these messages - are buffered on the MCU and sent only at a limited - rate (e.g. 10 Hz). - - severity : Severity of status. Relies on the definitions within RFC-5424. See enum MAV_SEVERITY. (uint8_t) - text : Status text message, without null termination character (char) - - ''' - return self.send(self.statustext_encode(severity, text)) - - def debug_encode(self, time_boot_ms, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - msg = MAVLink_debug_message(time_boot_ms, ind, value) - msg.pack(self) - return msg - - def debug_send(self, time_boot_ms, ind, value): - ''' - Send a debug value. The index is used to discriminate between values. - These values show up in the plot of QGroundControl as - DEBUG N. - - time_boot_ms : Timestamp (milliseconds since system boot) (uint32_t) - ind : index of debug variable (uint8_t) - value : DEBUG value (float) - - ''' - return self.send(self.debug_encode(time_boot_ms, ind, value)) - diff --git a/Tools/autotest/pymavlink/mavutil.py b/Tools/autotest/pymavlink/mavutil.py deleted file mode 100644 index 3f0a013444..0000000000 --- a/Tools/autotest/pymavlink/mavutil.py +++ /dev/null @@ -1,889 +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, fcntl -from math import * -from mavextra import * - -if os.getenv('MAVLINK10') or 'MAVLINK10' in os.environ: - 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 - -mavfile_global = None - -class location(object): - '''represent a GPS coordinate''' - def __init__(self, lat, lng, alt=0, heading=0): - self.lat = lat - self.lng = lng - self.alt = alt - self.heading = heading - - def __str__(self): - return "lat=%.6f,lon=%.6f,alt=%.1f" % (self.lat, self.lng, self.alt) - -class mavfile(object): - '''a generic mavlink port''' - def __init__(self, fd, address, source_system=255, notimestamps=False, input=True): - global mavfile_global - if input: - mavfile_global = self - 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.target_system = 0 - self.target_component = 0 - self.source_system = source_system - self.first_byte = True - self.robust_parsing = True - self.mav = mavlink.MAVLink(self, srcSystem=self.source_system) - self.mav.robust_parsing = self.robust_parsing - 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.uptime = 0.0 - self.notimestamps = notimestamps - self._timestamp = None - self.ground_pressure = None - self.ground_temperature = None - self.altitude = 0 - self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION - self.last_seq = -1 - self.mav_loss = 0 - self.mav_count = 0 - self.stop_on_EOF = False - - def auto_mavlink_version(self, buf): - '''auto-switch mavlink protocol version''' - global mavlink - if len(buf) == 0: - return - if not ord(buf[0]) in [ 85, 254 ]: - return - self.first_byte = False - if self.WIRE_PROTOCOL_VERSION == "0.9" and ord(buf[0]) == 254: - import mavlinkv10 as mavlink - os.environ['MAVLINK10'] = '1' - elif self.WIRE_PROTOCOL_VERSION == "1.0" and ord(buf[0]) == 85: - import mavlink as mavlink - else: - return - # switch protocol - (callback, callback_args, callback_kwargs) = (self.mav.callback, - self.mav.callback_args, - self.mav.callback_kwargs) - self.mav = mavlink.MAVLink(self, srcSystem=self.source_system) - self.mav.robust_parsing = self.robust_parsing - self.WIRE_PROTOCOL_VERSION = mavlink.WIRE_PROTOCOL_VERSION - (self.mav.callback, self.mav.callback_args, self.mav.callback_kwargs) = (callback, - callback_args, - callback_kwargs) - - 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''' - if '_posted' in msg.__dict__: - return - msg._posted = True - msg._timestamp = time.time() - type = msg.get_type() - self.messages[type] = msg - - if 'usec' in msg.__dict__: - self.uptime = msg.usec * 1.0e-6 - - if self._timestamp is not None: - if self.notimestamps: - msg._timestamp = self.uptime - else: - msg._timestamp = self._timestamp - - if not ( - # its the radio or planner - (msg.get_srcSystem() == ord('3') and msg.get_srcComponent() == ord('D')) or - msg.get_srcSystem() == 255 or - msg.get_type() == 'BAD_DATA'): - seq = (self.last_seq+1) % 256 - seq2 = msg.get_seq() - if seq != seq2 and self.last_seq != -1: - diff = (seq2 - seq) % 256 - self.mav_loss += diff - self.last_seq = seq2 - self.mav_count += 1 - - 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': - s = str(msg.param_id) - 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 - elif type == 'GPS_RAW_INT': - if self.messages['HOME'].fix_type < 3: - self.messages['HOME'] = msg - for hook in self.message_hooks: - hook(self, msg) - - - def packet_loss(self): - '''packet loss as a percentage''' - if self.mav_count == 0: - return 0 - return (100.0*self.mav_loss)/(self.mav_count+self.mav_loss) - - - 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 or self.stop_on_EOF): - return None - if self.logfile_raw: - self.logfile_raw.write(str(s)) - if self.first_byte: - self.auto_mavlink_version(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 mavlink10(self): - '''return True if using MAVLink 1.0''' - return self.WIRE_PROTOCOL_VERSION == "1.0" - - 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 self.mavlink10(): - if parm_type == None: - parm_type = mavlink.MAVLINK_TYPE_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 self.mavlink10(): - 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 self.mavlink10(): - 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 self.mavlink10(): - 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 self.mavlink10(): - 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_current(self): - '''return current waypoint''' - if self.mavlink10(): - m = self.recv_match(type='MISSION_CURRENT', blocking=True) - else: - m = self.recv_match(type='WAYPOINT_CURRENT', blocking=True) - return m.seq - - def waypoint_count_send(self, seq): - '''wrapper for waypoint_count_send''' - if self.mavlink10(): - 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) - - def set_mode_auto(self): - '''enter auto mode''' - if self.mavlink10(): - self.mav.command_long_send(self.target_system, self.target_component, - mavlink.MAV_CMD_MISSION_START, 0, 0, 0, 0, 0, 0, 0, 0) - else: - MAV_ACTION_SET_AUTO = 13 - self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_SET_AUTO) - - def set_mode_rtl(self): - '''enter RTL mode''' - if self.mavlink10(): - self.mav.command_long_send(self.target_system, self.target_component, - mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0) - else: - MAV_ACTION_RETURN = 3 - self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_RETURN) - - def set_mode_loiter(self): - '''enter LOITER mode''' - if self.mavlink10(): - self.mav.command_long_send(self.target_system, self.target_component, - mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0, 0, 0, 0) - else: - MAV_ACTION_LOITER = 27 - self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_LOITER) - - def calibrate_imu(self): - '''calibrate IMU''' - if self.mavlink10(): - self.mav.command_long_send(self.target_system, self.target_component, - mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0, - 1, 1, 1, 1, 0, 0, 0) - else: - MAV_ACTION_CALIBRATE_GYRO = 17 - self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_CALIBRATE_GYRO) - - def calibrate_level(self): - '''calibrate accels''' - if self.mavlink10(): - self.mav.command_long_send(self.target_system, self.target_component, - mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, 0, - 1, 1, 1, 1, 0, 0, 0) - else: - MAV_ACTION_CALIBRATE_ACC = 19 - self.mav.action_send(self.target_system, self.target_component, MAV_ACTION_CALIBRATE_ACC) - - def wait_gps_fix(self): - self.recv_match(type='VFR_HUD', blocking=True) - if self.mavlink10(): - self.recv_match(type='GPS_RAW_INT', blocking=True, - condition='GPS_RAW_INT.fix_type==3 and GPS_RAW_INT.lat != 0 and GPS_RAW_INT.alt != 0') - else: - self.recv_match(type='GPS_RAW', blocking=True, - condition='GPS_RAW.fix_type==2 and GPS_RAW.lat != 0 and GPS_RAW.alt != 0') - - def location(self): - '''return current location''' - self.wait_gps_fix() - # wait for another VFR_HUD, to ensure we have correct altitude - self.recv_match(type='VFR_HUD', blocking=True) - if self.mavlink10(): - return location(self.messages['GPS_RAW_INT'].lat*1.0e-7, - self.messages['GPS_RAW_INT'].lon*1.0e-7, - self.messages['VFR_HUD'].alt, - self.messages['VFR_HUD'].heading) - else: - return location(self.messages['GPS_RAW'].lat, - self.messages['GPS_RAW'].lon, - self.messages['VFR_HUD'].alt, - self.messages['VFR_HUD'].heading) - - def field(self, type, field, default=None): - '''convenient function for returning an arbitrary MAVLink - field with a default''' - if not type in self.messages: - return default - return getattr(self.messages[type], field, default) - - def param(self, name, default=None): - '''convenient function for returning an arbitrary MAVLink - parameter with a default''' - if not name in self.params: - return default - return self.params[name] - -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() - flags = fcntl.fcntl(fd, fcntl.F_GETFD) - flags |= fcntl.FD_CLOEXEC - fcntl.fcntl(fd, fcntl.F_SETFD, flags) - 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 Exception: - 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(0.5) - - -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.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - self.port.bind((a[0], int(a[1]))) - else: - self.destination_addr = (a[0], int(a[1])) - flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD) - flags |= fcntl.FD_CLOEXEC - fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags) - self.port.setblocking(0) - self.last_address = None - mavfile.__init__(self, self.port.fileno(), device, source_system=source_system, input=input) - - 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, errno.ECONNREFUSED ]: - 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 - if self.first_byte: - self.auto_mavlink_version(s) - 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) - flags = fcntl.fcntl(self.port.fileno(), fcntl.F_GETFD) - flags |= fcntl.FD_CLOEXEC - fcntl.fcntl(self.port.fileno(), fcntl.F_SETFD, flags) - 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() - self.stop_on_EOF = True - - 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 - if self.filesize != 0: - 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 - self._link = 0 - else: - tbuf = self.f.read(8) - if len(tbuf) != 8: - return - (tusec,) = struct.unpack('>Q', tbuf) - t = tusec * 1.0e-6 - self._link = tusec & 0x3 - 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_apm = { - 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' - } - mapping_acm = { - 0 : 'STABILIZE', - 1 : 'ACRO', - 2 : 'ALT_HOLD', - 3 : 'AUTO', - 4 : 'GUIDED', - 5 : 'LOITER', - 6 : 'RTL', - 7 : 'CIRCLE', - 8 : 'POSITION', - 9 : 'LAND', - 10 : 'OF_LOITER', - 11 : 'APPROACH' - } - if msg.type == mavlink.MAV_TYPE_QUADROTOR: - if msg.custom_mode in mapping_acm: - return mapping_acm[msg.custom_mode] - if msg.type == mavlink.MAV_TYPE_FIXED_WING: - if msg.custom_mode in mapping_apm: - return mapping_apm[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 diff --git a/Tools/autotest/pymavlink/mavwp.py b/Tools/autotest/pymavlink/mavwp.py deleted file mode 100644 index 58bd0ec434..0000000000 --- a/Tools/autotest/pymavlink/mavwp.py +++ /dev/null @@ -1,209 +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 set(self, w, idx): - '''set a waypoint''' - w.seq = idx - if w.seq == self.count(): - return self.add(w) - if self.count() <= idx: - raise MAVWPError('adding waypoint at idx=%u past end of list (count=%u)' % (idx, self.count())) - self.wpoints[idx] = 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() diff --git a/Tools/autotest/pymavlink/scanwin32.py b/Tools/autotest/pymavlink/scanwin32.py deleted file mode 100644 index f90ed5f4d8..0000000000 --- a/Tools/autotest/pymavlink/scanwin32.py +++ /dev/null @@ -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) diff --git a/Tools/autotest/pysim/sim_multicopter.py b/Tools/autotest/pysim/sim_multicopter.py index a4eb1fc342..72ca51e665 100755 --- a/Tools/autotest/pysim/sim_multicopter.py +++ b/Tools/autotest/pysim/sim_multicopter.py @@ -5,7 +5,7 @@ import util, time, os, sys, math import socket, struct import select, errno -sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', 'pymavlink')) +sys.path.insert(0, os.path.join(os.path.dirname(os.path.realpath(__file__)), '..', '..', '..', '..', 'mavlink', 'pymavlink')) import fgFDM def sim_send(m, a):