mirror of https://github.com/ArduPilot/ardupilot
511 lines
18 KiB
Python
Executable File
511 lines
18 KiB
Python
Executable File
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
"""
|
|
author: Alex Apostoli
|
|
based on https://github.com/hkm95/python-multiwii
|
|
which is under GPLv3
|
|
"""
|
|
|
|
import struct
|
|
import time
|
|
import sys
|
|
import re
|
|
|
|
class MSPItem:
|
|
def __init__(self, name, fmt, fields):
|
|
self.name = name
|
|
self.format = fmt
|
|
self.fields = fields
|
|
if not isinstance(self.format, list):
|
|
self.format = [self.format]
|
|
self.fields = [self.fields]
|
|
self.values = {}
|
|
|
|
def parse(self, msp, dataSize):
|
|
'''parse data'''
|
|
ofs = msp.p
|
|
for i in range(len(self.format)):
|
|
fmt = self.format[i]
|
|
fields = self.fields[i].split(',')
|
|
if fmt[0] == '{':
|
|
# we have a repeat count from an earlier variable
|
|
right = fmt.find('}')
|
|
vname = fmt[1:right]
|
|
count = self.values[vname]
|
|
fmt = "%u%s" % (count, fmt[right+1:])
|
|
if fmt[0].isdigit():
|
|
repeat = int(re.search(r'\d+', fmt).group())
|
|
else:
|
|
repeat = None
|
|
fmt = "<" + fmt
|
|
fmt_size = struct.calcsize(fmt)
|
|
if dataSize < fmt_size:
|
|
raise Exception("Format %s needs %u bytes got %u for %s" % (self.name, fmt_size, dataSize, fmt))
|
|
values = list(struct.unpack(fmt, msp.inBuf[ofs:ofs+fmt_size]))
|
|
if repeat is not None:
|
|
for i in range(len(fields)):
|
|
self.values[fields[i]] = []
|
|
for j in range(repeat):
|
|
self.values[fields[i]].append(values[j*len(fields)])
|
|
else:
|
|
for i in range(len(fields)):
|
|
self.values[fields[i]] = values[i]
|
|
dataSize -= fmt_size
|
|
ofs += fmt_size
|
|
msp.by_name[self.name] = self
|
|
#print("Got %s" % self.name)
|
|
|
|
class PyMSP:
|
|
""" Multiwii Serial Protocol """
|
|
OSD_RSSI_VALUE = 0
|
|
OSD_MAIN_BATT_VOLTAGE = 1
|
|
OSD_CROSSHAIRS = 2
|
|
OSD_ARTIFICIAL_HORIZON = 3
|
|
OSD_HORIZON_SIDEBARS = 4
|
|
OSD_ITEM_TIMER_1 = 5
|
|
OSD_ITEM_TIMER_2 = 6
|
|
OSD_FLYMODE = 7
|
|
OSD_CRAFT_NAME = 8
|
|
OSD_THROTTLE_POS = 9
|
|
OSD_VTX_CHANNEL = 10
|
|
OSD_CURRENT_DRAW = 11
|
|
OSD_MAH_DRAWN = 12
|
|
OSD_GPS_SPEED = 13
|
|
OSD_GPS_SATS = 14
|
|
OSD_ALTITUDE = 15
|
|
OSD_ROLL_PIDS = 16
|
|
OSD_PITCH_PIDS = 17
|
|
OSD_YAW_PIDS = 18
|
|
OSD_POWER = 19
|
|
OSD_PIDRATE_PROFILE = 20
|
|
OSD_WARNINGS = 21
|
|
OSD_AVG_CELL_VOLTAGE = 22
|
|
OSD_GPS_LON = 23
|
|
OSD_GPS_LAT = 24
|
|
OSD_DEBUG = 25
|
|
OSD_PITCH_ANGLE = 26
|
|
OSD_ROLL_ANGLE = 27
|
|
OSD_MAIN_BATT_USAGE = 28
|
|
OSD_DISARMED = 29
|
|
OSD_HOME_DIR = 30
|
|
OSD_HOME_DIST = 31
|
|
OSD_NUMERICAL_HEADING = 32
|
|
OSD_NUMERICAL_VARIO = 33
|
|
OSD_COMPASS_BAR = 34
|
|
OSD_ESC_TMP = 35
|
|
OSD_ESC_RPM = 36
|
|
OSD_REMAINING_TIME_ESTIMATE = 37
|
|
OSD_RTC_DATETIME = 38
|
|
OSD_ADJUSTMENT_RANGE = 39
|
|
OSD_CORE_TEMPERATURE = 40
|
|
OSD_ANTI_GRAVITY = 41
|
|
OSD_G_FORCE = 42
|
|
OSD_MOTOR_DIAG = 43
|
|
OSD_LOG_STATUS = 44
|
|
OSD_FLIP_ARROW = 45
|
|
OSD_LINK_QUALITY = 46
|
|
OSD_FLIGHT_DIST = 47
|
|
OSD_STICK_OVERLAY_LEFT = 48
|
|
OSD_STICK_OVERLAY_RIGHT = 49
|
|
OSD_DISPLAY_NAME = 50
|
|
OSD_ESC_RPM_FREQ = 51
|
|
OSD_RATE_PROFILE_NAME = 52
|
|
OSD_PID_PROFILE_NAME = 53
|
|
OSD_PROFILE_NAME = 54
|
|
OSD_RSSI_DBM_VALUE = 55
|
|
OSD_RC_CHANNELS = 56
|
|
OSD_CAMERA_FRAME = 57
|
|
|
|
MSP_NAME =10
|
|
MSP_OSD_CONFIG =84
|
|
MSP_IDENT =100
|
|
MSP_STATUS =101
|
|
MSP_RAW_IMU =102
|
|
MSP_SERVO =103
|
|
MSP_MOTOR =104
|
|
MSP_RC =105
|
|
MSP_RAW_GPS =106
|
|
MSP_COMP_GPS =107
|
|
MSP_ATTITUDE =108
|
|
MSP_ALTITUDE =109
|
|
MSP_ANALOG =110
|
|
MSP_RC_TUNING =111
|
|
MSP_PID =112
|
|
MSP_BOX =113
|
|
MSP_MISC =114
|
|
MSP_MOTOR_PINS =115
|
|
MSP_BOXNAMES =116
|
|
MSP_PIDNAMES =117
|
|
MSP_WP =118
|
|
MSP_BOXIDS =119
|
|
MSP_SERVO_CONF =120
|
|
MSP_NAV_STATUS =121
|
|
MSP_NAV_CONFIG =122
|
|
MSP_MOTOR_3D_CONFIG =124
|
|
MSP_RC_DEADBAND =125
|
|
MSP_SENSOR_ALIGNMENT =126
|
|
MSP_LED_STRIP_MODECOLOR =127
|
|
MSP_VOLTAGE_METERS =128
|
|
MSP_CURRENT_METERS =129
|
|
MSP_BATTERY_STATE =130
|
|
MSP_MOTOR_CONFIG =131
|
|
MSP_GPS_CONFIG =132
|
|
MSP_COMPASS_CONFIG =133
|
|
MSP_ESC_SENSOR_DATA =134
|
|
MSP_GPS_RESCUE =135
|
|
MSP_GPS_RESCUE_PIDS =136
|
|
MSP_VTXTABLE_BAND =137
|
|
MSP_VTXTABLE_POWERLEVEL =138
|
|
MSP_MOTOR_TELEMETRY =139
|
|
|
|
MSP_SET_RAW_RC =200
|
|
MSP_SET_RAW_GPS =201
|
|
MSP_SET_PID =202
|
|
MSP_SET_BOX =203
|
|
MSP_SET_RC_TUNING =204
|
|
MSP_ACC_CALIBRATION =205
|
|
MSP_MAG_CALIBRATION =206
|
|
MSP_SET_MISC =207
|
|
MSP_RESET_CONF =208
|
|
MSP_SET_WP =209
|
|
MSP_SELECT_SETTING =210
|
|
MSP_SET_HEAD =211
|
|
MSP_SET_SERVO_CONF =212
|
|
MSP_SET_MOTOR =214
|
|
MSP_SET_NAV_CONFIG =215
|
|
MSP_SET_MOTOR_3D_CONFIG =217
|
|
MSP_SET_RC_DEADBAND =218
|
|
MSP_SET_RESET_CURR_PID =219
|
|
MSP_SET_SENSOR_ALIGNMENT =220
|
|
MSP_SET_LED_STRIP_MODECOLOR=221
|
|
MSP_SET_MOTOR_CONFIG =222
|
|
MSP_SET_GPS_CONFIG =223
|
|
MSP_SET_COMPASS_CONFIG =224
|
|
MSP_SET_GPS_RESCUE =225
|
|
MSP_SET_GPS_RESCUE_PIDS =226
|
|
MSP_SET_VTXTABLE_BAND =227
|
|
MSP_SET_VTXTABLE_POWERLEVEL=228
|
|
|
|
|
|
MSP_BIND =241
|
|
MSP_RTC =247
|
|
|
|
MSP_EEPROM_WRITE =250
|
|
|
|
MSP_DEBUGMSG =253
|
|
MSP_DEBUG =254
|
|
|
|
IDLE = 0
|
|
HEADER_START = 1
|
|
HEADER_M = 2
|
|
HEADER_ARROW = 3
|
|
HEADER_SIZE = 4
|
|
HEADER_CMD = 5
|
|
HEADER_ERR = 6
|
|
|
|
PIDITEMS = 10
|
|
|
|
MESSAGES = {
|
|
MSP_RAW_GPS: MSPItem('RAW_GPS', "BBiihH", "fix,numSat,Lat,Lon,Alt,Speed"),
|
|
MSP_IDENT: MSPItem('IDENT', "BBBI", "version,multiType,MSPVersion,multiCapability"),
|
|
MSP_STATUS: MSPItem('STATUS', "HHHI", "cycleTime,i2cError,present,mode"),
|
|
MSP_RAW_IMU: MSPItem('RAW_IMU', "hhhhhhhhh", "AccX,AccY,AccZ,GyrX,GyrY,GyrZ,MagX,MagY,MagZ"),
|
|
MSP_SERVO: MSPItem('SERVO', "8h", "servo"),
|
|
MSP_MOTOR: MSPItem('MOTOR', "8h", "motor"),
|
|
MSP_RC: MSPItem('RC', "8h", "rc"),
|
|
MSP_COMP_GPS: MSPItem('COMP_GPS', "HhB", "distanceToHome,directionToHome,update"),
|
|
MSP_ATTITUDE: MSPItem('ATTITUDE', "hhh", "roll,pitch,yaw"),
|
|
MSP_ALTITUDE: MSPItem('ALTITUDE', "ih", "alt,vspeed"),
|
|
MSP_RC_TUNING: MSPItem('RC_TUNING', "BBBBBBB", "RC_Rate,RC_Expo,RollPitchRate,YawRate,DynThrPID,ThrottleMID,ThrottleExpo"),
|
|
MSP_BATTERY_STATE: MSPItem('BATTERY_STATE', "BHBHh", "cellCount,capacity,voltage,mah,current"),
|
|
MSP_RTC: MSPItem('RTC', "HBBBBBH", "year,mon,mday,hour,min,sec,millis"),
|
|
MSP_OSD_CONFIG: MSPItem("OSD_CONFIG",
|
|
["BBBBHBBH",
|
|
"{osd_item_count}H",
|
|
"B", "{stats_item_count}H",
|
|
"B", "{timer_count}H",
|
|
"HBIBBB"],
|
|
["feature,video_system,units,rssi_alarm,cap_alarm,unused1,osd_item_count,alt_alarm",
|
|
"osd_items",
|
|
"stats_item_count", "stats_items",
|
|
"timer_count", "timer_items",
|
|
"legacy_warnings,warnings_count,enabled_warnings,profiles,selected_profile,osd_overlay"]),
|
|
MSP_PID: MSPItem("PID", "8PID", "P,I,D"),
|
|
MSP_MISC: MSPItem("MISC", "HHHHHII","intPowerTrigger,conf1,conf2,conf3,conf4,conf5,conf6"),
|
|
MSP_MOTOR_PINS: MSPItem("MOTOR_PINS", "8H","MP"),
|
|
MSP_ANALOG: MSPItem("ANALOG", "BHHHH", "dV,consumed_mah,rssi,current,volt"),
|
|
MSP_STATUS: MSPItem("STATUS", "HHHIBHHBBIB", "task_delta,i2c_err_count,sensor_status,mode_flags,nop_1,system_load,gyro_time,nop_2,nop_3,armed,extra"),
|
|
}
|
|
|
|
def __init__(self):
|
|
|
|
self.msp_name = {
|
|
'name':None
|
|
}
|
|
self.msp_osd_config = {}
|
|
|
|
self.inBuf = bytearray([0] * 255)
|
|
self.p = 0
|
|
self.c_state = self.IDLE
|
|
self.err_rcvd = False
|
|
self.checksum = 0
|
|
self.cmd = 0
|
|
self.offset=0
|
|
self.dataSize=0
|
|
self.servo = []
|
|
self.mot = []
|
|
self.RCChan = []
|
|
self.byteP = []
|
|
self.byteI = []
|
|
self.byteD = []
|
|
self.confINF = []
|
|
self.byteMP = []
|
|
|
|
self.confP = []
|
|
self.confI = []
|
|
self.confD = []
|
|
|
|
# parsed messages, indexed by name
|
|
self.by_name = {}
|
|
|
|
def get(self, fieldname):
|
|
'''get a field from a parsed message by Message.Field name'''
|
|
a = fieldname.split('.')
|
|
msgName = a[0]
|
|
fieldName = a[1]
|
|
if not msgName in self.by_name:
|
|
# default to zero for simplicty of display
|
|
return 0
|
|
msg = self.by_name[msgName]
|
|
if not fieldName in msg.values:
|
|
raise Exception("Unknown field %s" % fieldName)
|
|
return msg.values[fieldName]
|
|
|
|
def read32(self):
|
|
'''signed 32 bit number'''
|
|
value, = struct.unpack("<i", self.inBuf[self.p:self.p+4])
|
|
self.p += 4
|
|
return value
|
|
|
|
def read32u(self):
|
|
'''unsigned 32 bit number'''
|
|
value, = struct.unpack("<I", self.inBuf[self.p:self.p+4])
|
|
self.p += 4
|
|
return value
|
|
|
|
def read16(self):
|
|
'''signed 16 bit number'''
|
|
value, = struct.unpack("<h", self.inBuf[self.p:self.p+2])
|
|
self.p += 2
|
|
return value
|
|
|
|
def read16u(self):
|
|
'''unsigned 16 bit number'''
|
|
value, = struct.unpack("<H", self.inBuf[self.p:self.p+2])
|
|
self.p += 2
|
|
return value
|
|
|
|
def read8(self):
|
|
'''unsigned 8 bit number'''
|
|
value, = struct.unpack("<B", self.inBuf[self.p:self.p+1])
|
|
self.p += 1
|
|
return value
|
|
|
|
def requestMSP (self, msp, payload = [], payloadinbytes = False):
|
|
|
|
if msp < 0:
|
|
return 0
|
|
checksum = 0
|
|
bf = ['$', 'M', '<']
|
|
|
|
pl_size = 2 * ((len(payload)) & 0xFF)
|
|
bf.append(pl_size)
|
|
checksum ^= (pl_size&0xFF)
|
|
|
|
bf.append(msp&0xFF)
|
|
checksum ^= (msp&0xFF)
|
|
if payload > 0:
|
|
if (payloadinbytes == False):
|
|
for c in struct.pack('<%dh' % ((pl_size) / 2), *payload):
|
|
checksum ^= (ord(c) & 0xFF)
|
|
else:
|
|
for c in struct.pack('<%Bh' % ((pl_size) / 2), *payload):
|
|
checksum ^= (ord(c) & 0xFF)
|
|
bf = bf + payload
|
|
bf.append(checksum)
|
|
return bf
|
|
|
|
def evaluateCommand(self, cmd, dataSize):
|
|
if cmd in self.MESSAGES:
|
|
# most messages are parsed from the MESSAGES list
|
|
self.MESSAGES[cmd].parse(self, dataSize)
|
|
elif cmd == self.MSP_NAME:
|
|
s = bytearray()
|
|
for i in range(0,dataSize,1):
|
|
b = self.read8()
|
|
if b == 0:
|
|
break
|
|
s.append(b)
|
|
self.msp_name['name'] = s.decode("utf-8")
|
|
|
|
elif cmd == self.MSP_ACC_CALIBRATION:
|
|
x = None
|
|
elif cmd == self.MSP_MAG_CALIBRATION:
|
|
x = None
|
|
elif cmd == self.MSP_BOX:
|
|
x = None
|
|
elif cmd == self.MSP_BOXNAMES:
|
|
x = None
|
|
elif cmd == self.MSP_PIDNAMES:
|
|
x = None
|
|
elif cmd == self.MSP_SERVO_CONF:
|
|
x = None
|
|
elif cmd == self.MSP_DEBUGMSG:
|
|
x = None
|
|
elif cmd == self.MSP_DEBUG:
|
|
x = None
|
|
else:
|
|
print("Unhandled command ", cmd, dataSize)
|
|
|
|
def parseMspData(self, buf):
|
|
for c in buf:
|
|
self.parseMspByte(c)
|
|
|
|
def parseMspByte(self, c):
|
|
if sys.version_info.major >= 3:
|
|
cc = chr(c)
|
|
ci = c
|
|
else:
|
|
cc = c
|
|
ci = ord(c)
|
|
if self.c_state == self.IDLE:
|
|
if cc == '$':
|
|
self.c_state = self.HEADER_START
|
|
else:
|
|
self.c_state = self.IDLE
|
|
elif self.c_state == self.HEADER_START:
|
|
if cc == 'M':
|
|
self.c_state = self.HEADER_M
|
|
else:
|
|
self.c_state = self.IDLE
|
|
elif self.c_state == self.HEADER_M:
|
|
if cc == '>':
|
|
self.c_state = self.HEADER_ARROW
|
|
elif cc == '!':
|
|
self.c_state = self.HEADER_ERR
|
|
else:
|
|
self.c_state = self.IDLE
|
|
|
|
elif self.c_state == self.HEADER_ARROW or self.c_state == self.HEADER_ERR:
|
|
self.err_rcvd = (self.c_state == self.HEADER_ERR)
|
|
#print (struct.unpack('<B',c)[0])
|
|
self.dataSize = ci
|
|
# reset index variables
|
|
self.p = 0
|
|
self.offset = 0
|
|
self.checksum = 0
|
|
self.checksum ^= ci
|
|
# the command is to follow
|
|
self.c_state = self.HEADER_SIZE
|
|
elif self.c_state == self.HEADER_SIZE:
|
|
#print (struct.unpack('<B',c)[0])
|
|
self.cmd = ci
|
|
self.checksum ^= ci
|
|
self.c_state = self.HEADER_CMD
|
|
elif self.c_state == self.HEADER_CMD and self.offset < self.dataSize:
|
|
#print (struct.unpack('<B',c)[0])
|
|
self.checksum ^= ci
|
|
self.inBuf[self.offset] = ci
|
|
self.offset += 1
|
|
elif self.c_state == self.HEADER_CMD and self.offset >= self.dataSize:
|
|
# compare calculated and transferred checksum
|
|
if ((self.checksum&0xFF) == ci):
|
|
if self.err_rcvd:
|
|
print("Vehicle didn't understand the request type")
|
|
else:
|
|
self.evaluateCommand(self.cmd, self.dataSize)
|
|
else:
|
|
print('"invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF))')
|
|
|
|
self.c_state = self.IDLE
|
|
|
|
def setPID(self):
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_PID))
|
|
self.receiveData(self.MSP_PID)
|
|
time.sleep(0.04)
|
|
payload = []
|
|
for i in range(0, self.PIDITEMS, 1):
|
|
self.byteP[i] = int((round(self.confP[i] * 10)))
|
|
self.byteI[i] = int((round(self.confI[i] * 1000)))
|
|
self.byteD[i] = int((round(self.confD[i])))
|
|
|
|
|
|
# POS - 4 POSR - 5 NAVR - 6
|
|
|
|
self.byteP[4] = int((round(self.confP[4] * 100.0)))
|
|
self.byteI[4] = int((round(self.confI[4] * 100.0)))
|
|
self.byteP[5] = int((round(self.confP[5] * 10.0)))
|
|
self.byteI[5] = int((round(self.confI[5] * 100.0)))
|
|
self.byteD[5] = int((round(self.confD[5] * 10000.0))) / 10
|
|
|
|
self.byteP[6] = int((round(self.confP[6] * 10.0)))
|
|
self.byteI[6] = int((round(self.confI[6] * 100.0)))
|
|
self.byteD[6] = int((round(self.confD[6] * 10000.0))) / 10
|
|
|
|
for i in range(0, self.PIDITEMS, 1):
|
|
payload.append(self.byteP[i])
|
|
payload.append(self.byteI[i])
|
|
payload.append(self.byteD[i])
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_SET_PID, payload, True), True)
|
|
|
|
|
|
def arm(self):
|
|
timer = 0
|
|
start = time.time()
|
|
while timer < 0.5:
|
|
data = [1500,1500,2000,1000]
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_SET_RAW_RC,data))
|
|
time.sleep(0.05)
|
|
timer = timer + (time.time() - start)
|
|
start = time.time()
|
|
|
|
def disarm(self):
|
|
timer = 0
|
|
start = time.time()
|
|
while timer < 0.5:
|
|
data = [1500,1500,1000,1000]
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_SET_RAW_RC,data))
|
|
time.sleep(0.05)
|
|
timer = timer + (time.time() - start)
|
|
start = time.time()
|
|
|
|
|
|
def receiveIMU(self, duration):
|
|
timer = 0
|
|
start = time.time()
|
|
while timer < duration:
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_RAW_IMU))
|
|
self.receiveData(self.MSP_RAW_IMU)
|
|
if self.msp_raw_imu['accx'] > 32768: # 2^15 ...to check if negative number is received
|
|
self.msp_raw_imu['accx'] -= 65536 # 2^16 ...converting into 2's complement
|
|
if self.msp_raw_imu['accy'] > 32768:
|
|
self.msp_raw_imu['accy'] -= 65536
|
|
if self.msp_raw_imu['accz'] > 32768:
|
|
self.msp_raw_imu['accz'] -= 65536
|
|
if self.msp_raw_imu['gyrx'] > 32768:
|
|
self.msp_raw_imu['gyrx'] -= 65536
|
|
if self.msp_raw_imu['gyry'] > 32768:
|
|
self.msp_raw_imu['gyry'] -= 65536
|
|
if self.msp_raw_imu['gyrz'] > 32768:
|
|
self.msp_raw_imu['gyrz'] -= 65536
|
|
print("size: %d, accx: %f, accy: %f, accz: %f, gyrx: %f, gyry: %f, gyrz: %f " %(self.msp_raw_imu['size'], self.msp_raw_imu['accx'], self.msp_raw_imu['accy'], self.msp_raw_imu['accz'], self.msp_raw_imu['gyrx'], self.msp_raw_imu['gyry'], self.msp_raw_imu['gyrz']))
|
|
time.sleep(0.04)
|
|
timer = timer + (time.time() - start)
|
|
start = time.time()
|
|
|
|
|
|
def calibrateIMU(self):
|
|
self.sendRequestMSP(self.requestMSP(self.MSP_ACC_CALIBRATION))
|
|
time.sleep(0.01)
|