mirror of https://github.com/ArduPilot/ardupilot
autotest: updated pymavlink to latest
This commit is contained in:
Andrew Tridgell
parent
21886104a6
commit
dce18f89f2
|
|
@ -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)
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
Loading…
Reference in New Issue