diff --git a/Tools/autotest/pymavlink/fgFDM.py b/Tools/autotest/pymavlink/fgFDM.py new file mode 100644 index 0000000000..ae1602e534 --- /dev/null +++ b/Tools/autotest/pymavlink/fgFDM.py @@ -0,0 +1,209 @@ +#!/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 index b4bb585f0d..4a8eec08b8 100644 --- a/Tools/autotest/pymavlink/mavextra.py +++ b/Tools/autotest/pymavlink/mavextra.py @@ -9,30 +9,191 @@ Released under GNU GPL version 3 or later from math import * -def norm_heading(RAW_IMU, ATTITUDE, declination): - '''calculate heading from RAW_IMU and ATTITUDE''' - xmag = RAW_IMU.xmag - ymag = RAW_IMU.ymag - zmag = RAW_IMU.zmag - pitch = ATTITUDE.pitch - roll = ATTITUDE.roll - - headX = xmag*cos(pitch) + ymag*sin(roll)*sin(pitch) + zmag*cos(roll)*sin(pitch) - headY = ymag*cos(roll) - zmag*sin(roll) - heading = atan2(-headY, headX) - heading = fmod(degrees(heading) + declination + 360, 360) - return heading - -def TrueHeading(SERVO_OUTPUT_RAW): - rc3_min = 1060 - rc3_max = 1850 - p = float(SERVO_OUTPUT_RAW.servo3_raw - rc3_min) / (rc3_max - rc3_min) - return 172 + (1.0-p)*(326 - 172) - def kmh(mps): '''convert m/s to Km/h''' return mps*3.6 -def altitude(press_abs, ground_press=955.0, ground_temp=30): +def altitude(SCALED_PRESSURE): '''calculate barometric altitude''' - return log(ground_press/press_abs)*(ground_temp+273.15)*29271.267*0.001 + import mavutil + self = mavutil.mavfile_global + if self.ground_pressure is None or self.ground_temperature is None: + return 0 + scaling = self.ground_pressure / (SCALED_PRESSURE.press_abs*100.0) + temp = self.ground_temperature + 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 index ee8720bf6a..693afa2a06 100644 --- a/Tools/autotest/pymavlink/mavlink.py +++ b/Tools/autotest/pymavlink/mavlink.py @@ -203,6 +203,10 @@ MAVLINK_MSG_ID_MOUNT_STATUS = 158 MAVLINK_MSG_ID_FENCE_POINT = 160 MAVLINK_MSG_ID_FENCE_FETCH_POINT = 161 MAVLINK_MSG_ID_FENCE_STATUS = 162 +MAVLINK_MSG_ID_AHRS = 163 +MAVLINK_MSG_ID_SIMSTATE = 164 +MAVLINK_MSG_ID_HWSTATUS = 165 +MAVLINK_MSG_ID_RADIO = 166 MAVLINK_MSG_ID_HEARTBEAT = 0 MAVLINK_MSG_ID_BOOT = 1 MAVLINK_MSG_ID_SYSTEM_TIME = 2 @@ -486,6 +490,75 @@ class MAVLink_fence_status_message(MAVLink_message): 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 @@ -1784,6 +1857,10 @@ mavlink_map = { 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 ), @@ -2417,6 +2494,128 @@ class MAVLink(object): ''' 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. diff --git a/Tools/autotest/pymavlink/mavlinkv10.py b/Tools/autotest/pymavlink/mavlinkv10.py new file mode 100644 index 0000000000..0983079bbf --- /dev/null +++ b/Tools/autotest/pymavlink/mavlinkv10.py @@ -0,0 +1,5532 @@ +''' +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" + +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): + return self._msgbuf + + def get_header(self): + return self._header + + def get_payload(self): + return self._payload + + def get_crc(self): + return self._crc + + def get_fieldnames(self): + return self._fieldnames + + def get_type(self): + return self._type + + def get_msgId(self): + return self._header.msgId + + def get_srcSystem(self): + return self._header.srcSystem + + def get_srcComponent(self): + return self._header.srcComponent + + def get_seq(self): + return self._header.seq + + def __str__(self): + ret = '%s {' % self._type + for a in self._fieldnames: + v = getattr(self, a) + ret += '%s : %s, ' % (a, v) + ret = ret[0:-2] + '}' + return ret + + def pack(self, mav, crc_extra, payload): + self._payload = payload + self._header = MAVLink_header(self._header.msgId, len(payload), mav.seq, + mav.srcSystem, mav.srcComponent) + self._msgbuf = self._header.pack() + payload + crc = mavutil.x25crc(self._msgbuf[1:]) + if 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 (char) + 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 (char) + 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_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 (char) + param_value : Onboard parameter value (float) + param_type : Onboard parameter type: see MAV_VAR enum (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 (char) + param_value : Onboard parameter value (float) + param_type : Onboard parameter type: see MAV_VAR enum (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 (char) + param_value : Onboard parameter value (float) + param_type : Onboard parameter type: see MAV_VAR enum (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 (char) + param_value : Onboard parameter value (float) + param_type : Onboard parameter type: see MAV_VAR enum (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, target_systems, roll, pitch, yaw, thrust): + ''' + + + target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t) + roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t) + + ''' + msg = MAVLink_set_quad_swarm_roll_pitch_yaw_thrust_message(target_systems, roll, pitch, yaw, thrust) + msg.pack(self) + return msg + + def set_quad_swarm_roll_pitch_yaw_thrust_send(self, target_systems, roll, pitch, yaw, thrust): + ''' + + + target_systems : System IDs for 6 quadrotors: 0..5, the ID's are the MAVLink IDs (uint8_t) + roll : Desired roll angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + pitch : Desired pitch angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + yaw : Desired yaw angle in radians, scaled to int16 for 6 quadrotors: 0..5 (int16_t) + thrust : Collective thrust, scaled to uint16 for 6 quadrotors: 0..5 (uint16_t) + + ''' + return self.send(self.set_quad_swarm_roll_pitch_yaw_thrust_encode(target_systems, 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 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 index ab28f83708..d1ee4e7121 100644 --- a/Tools/autotest/pymavlink/mavutil.py +++ b/Tools/autotest/pymavlink/mavutil.py @@ -32,10 +32,13 @@ def evaluate_condition(condition, vars): return False return v +mavfile_global = None class mavfile(object): '''a generic mavlink port''' - def __init__(self, fd, address, source_system=255): + def __init__(self, fd, address, source_system=255, notimestamps=False): + global mavfile_global + mavfile_global = self self.fd = fd self.address = address self.messages = { 'MAV' : self } @@ -59,6 +62,13 @@ class mavfile(object): 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 def recv(self, n=None): '''default recv method''' @@ -81,6 +91,16 @@ class mavfile(object): 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 + self.timestamp = msg._timestamp if type == 'HEARTBEAT': self.target_system = msg.get_srcSystem() @@ -92,6 +112,10 @@ class mavfile(object): if msg.param_index+1 == msg.param_count: self.param_fetch_in_progress = False self.param_fetch_complete = True + if str(msg.param_id) == 'GND_ABS_PRESS': + self.ground_pressure = msg.param_value + if str(msg.param_id) == 'GND_TEMP': + self.ground_temperature = msg.param_value elif type == 'SYS_STATUS' and mavlink.WIRE_PROTOCOL_VERSION == '0.9': self.flightmode = mode_string_v09(msg) elif type == 'GPS_RAW': @@ -206,6 +230,52 @@ class mavfile(object): else: self.mav.waypoint_count_send(self.target_system, self.target_component, seq) + def set_mode_auto(self): + '''enter auto mode''' + if mavlink.WIRE_PROTOCOL_VERSION == '1.0': + 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 mavlink.WIRE_PROTOCOL_VERSION == '1.0': + 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 mavlink.WIRE_PROTOCOL_VERSION == '1.0': + 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 mavlink.WIRE_PROTOCOL_VERSION == '1.0': + 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 mavlink.WIRE_PROTOCOL_VERSION == '1.0': + 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) class mavserial(mavfile): '''a serial mavlink port''' @@ -357,7 +427,6 @@ class mavlogfile(mavfile): self.writeable = write self.robust_parsing = robust_parsing self.planner_format = planner_format - self.notimestamps = notimestamps self._two64 = math.pow(2.0, 63) mode = 'rb' if self.writeable: @@ -366,7 +435,13 @@ class mavlogfile(mavfile): else: mode = 'wb' self.f = open(filename, mode) - mavfile.__init__(self, None, filename, source_system=source_system) + self.filesize = os.path.getsize(filename) + self.percent = 0 + mavfile.__init__(self, None, filename, source_system=source_system, notimestamps=notimestamps) + if self.notimestamps: + self._timestamp = 0 + else: + self._timestamp = time.time() def close(self): self.f.close() @@ -382,6 +457,7 @@ class mavlogfile(mavfile): def pre_message(self): '''read timestamp if needed''' # read the timestamp + self.percent = (100.0 * self.f.tell()) / self.filesize if self.notimestamps: return if self.planner_format: @@ -403,10 +479,6 @@ class mavlogfile(mavfile): '''add timestamp to message''' # read the timestamp super(mavlogfile, self).post_message(msg) - if self.notimestamps: - msg._timestamp = time.time() - else: - msg._timestamp = self._timestamp if self.planner_format: self.f.read(1) # trailing newline self.timestamp = msg._timestamp @@ -467,6 +539,11 @@ class periodic_event(object): 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() @@ -601,6 +678,7 @@ def mode_string_v09(msg): (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", @@ -615,7 +693,7 @@ 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 = { + mapping_apm = { 0 : 'MANUAL', 1 : 'CIRCLE', 2 : 'STABILIZE', @@ -630,8 +708,26 @@ def mode_string_v10(msg): 15 : 'GUIDED', 16 : 'INITIALISING' } - if msg.custom_mode in mapping: - return mapping[msg.custom_mode] + 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 diff --git a/Tools/autotest/pymavlink/scanwin32.py b/Tools/autotest/pymavlink/scanwin32.py new file mode 100644 index 0000000000..f90ed5f4d8 --- /dev/null +++ b/Tools/autotest/pymavlink/scanwin32.py @@ -0,0 +1,236 @@ +#!/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)