zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_MSP: fixed viewer for py2 and py3

c415-sdk
Andrew Tridgell 5 years ago
parent
5a7f81b5c3
commit
05f4bf0b32
2 changed files with 33 additions and 46 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      libraries/AP_MSP/Tools/msposd.py
  2. 75
      libraries/AP_MSP/Tools/pymsp.py

4
libraries/AP_MSP/Tools/msposd.py
Unescape View File

@ -69,7 +69,7 @@ pygame.display.set_caption('MSP Display')
def item_to_pos(item): def item_to_pos(item):
'''map MSP item to a X,Y tuple or None''' '''map MSP item to a X,Y tuple or None'''
if item >= msp.msp_osd_config['osd_item_count']: if item is None or item >= msp.msp_osd_config.get('osd_item_count',0):
return None return None
pos = msp.msp_osd_config['osd_items'][item] pos = msp.msp_osd_config['osd_items'][item]
if pos < 2048: if pos < 2048:
@ -91,7 +91,7 @@ def display_text(item, message):
slen = len(message) slen = len(message)
px = X * FontWidth px = X * FontWidth
py = Y * FontHeight py = Y * FontHeight
textRect.center = (px+textRect.width/2, py+textRect.height/2) textRect.center = (px+textRect.width//2, py+textRect.height//2)
display_surface.blit(text, textRect) display_surface.blit(text, textRect)
def display_all(): def display_all():

75
libraries/AP_MSP/Tools/pymsp.py
Unescape View File

@ -8,6 +8,7 @@
import struct import struct
import time import time
import sys
class MSPItem: class MSPItem:
def __init__(self, name, fmt, fields): def __init__(self, name, fmt, fields):
@ -198,27 +199,7 @@ class PyMSP:
self.msp_name = { self.msp_name = {
'name':None 'name':None
} }
self.msp_osd_config = { self.msp_osd_config = {}
'feature':None, # 8
'video_system':None, # 8
'units':None, # 8
'rssi_alarm':None, # 8
'cap_alarm':None, # 16
'unusaed_1':None, # 8
'osd_item_count':None, # 8
'alt_alarm':None, # 16
'osd_items': [None] * 60, # x16
'stats_item_count':None, # 8
'stats_items': [None] * 30, # x16
'timer_count':None, # 8
'timer_items': [None] * 10, # 16
'legacy_warnings':None, # 16
'warnings_count':None, # 8
'enabled_warnings':None, # 32
'profiles':None, # 8
'selected_profile':None, # 8
'osd_overlay':None, # 8
}
self.inBuf = bytearray([0] * 255) self.inBuf = bytearray([0] * 255)
self.p = 0 self.p = 0
@ -271,19 +252,19 @@ class PyMSP:
def read16(self): def read16(self):
'''signed 16 bit number''' '''signed 16 bit number'''
value, = struct.unpack("<h", str(self.inBuf[self.p:self.p+2])) value, = struct.unpack("<h", self.inBuf[self.p:self.p+2])
self.p += 2 self.p += 2
return value return value
def read16u(self): def read16u(self):
'''unsigned 16 bit number''' '''unsigned 16 bit number'''
value, = struct.unpack("<H", str(self.inBuf[self.p:self.p+2])) value, = struct.unpack("<H", self.inBuf[self.p:self.p+2])
self.p += 2 self.p += 2
return value return value
def read8(self): def read8(self):
'''unsigned 8 bit number''' '''unsigned 8 bit number'''
value, = struct.unpack("<B", str(self.inBuf[self.p:self.p+1])) value, = struct.unpack("<B", self.inBuf[self.p:self.p+1])
self.p += 1 self.p += 1
return value return value
@ -388,13 +369,16 @@ class PyMSP:
self.msp_osd_config['unusaed_1'] = self.read8() # 8 self.msp_osd_config['unusaed_1'] = self.read8() # 8
self.msp_osd_config['osd_item_count'] = self.read8() # 8 self.msp_osd_config['osd_item_count'] = self.read8() # 8
self.msp_osd_config['alt_alarm'] = self.read16() # 16 self.msp_osd_config['alt_alarm'] = self.read16() # 16
for i in range(0, self.msp_osd_config['osd_item_count'], 1): self.msp_osd_config['osd_items'] = [0] * self.msp_osd_config['osd_item_count']
for i in range(self.msp_osd_config['osd_item_count']):
self.msp_osd_config['osd_items'][i] = self.read16u() # x 16 self.msp_osd_config['osd_items'][i] = self.read16u() # x 16
self.msp_osd_config['stats_item_count'] = self.read8() # 8 self.msp_osd_config['stats_item_count'] = self.read8() # 8
for i in range(0, self.msp_osd_config['stats_item_count'], 1): self.msp_osd_config['stats_items'] = [0] * self.msp_osd_config['stats_item_count']
for i in range(self.msp_osd_config['stats_item_count']):
self.msp_osd_config['stats_items'][i] = self.read16u() # x 16 self.msp_osd_config['stats_items'][i] = self.read16u() # x 16
self.msp_osd_config['timer_count'] = self.read8() # 8 self.msp_osd_config['timer_count'] = self.read8() # 8
for i in range(0, self.msp_osd_config['timer_count'], 1): self.msp_osd_config['timer_items'] = [0] * self.msp_osd_config['timer_count']
for i in range(self.msp_osd_config['timer_count']):
self.msp_osd_config['timer_items'][i] = self.read16u() # x 16 self.msp_osd_config['timer_items'][i] = self.read16u() # x 16
self.msp_osd_config['legacy_warnings'] = self.read16u() # 16 self.msp_osd_config['legacy_warnings'] = self.read16u() # 16
self.msp_osd_config['warnings_count'] = self.read8() # 8 self.msp_osd_config['warnings_count'] = self.read8() # 8
@ -411,20 +395,26 @@ class PyMSP:
self.parseMspByte(c) self.parseMspByte(c)
def parseMspByte(self, 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 self.c_state == self.IDLE:
if c == '$': if cc == '$':
self.c_state = self.HEADER_START self.c_state = self.HEADER_START
else: else:
self.c_state = self.IDLE self.c_state = self.IDLE
elif self.c_state == self.HEADER_START: elif self.c_state == self.HEADER_START:
if c == 'M': if cc == 'M':
self.c_state = self.HEADER_M self.c_state = self.HEADER_M
else: else:
self.c_state = self.IDLE self.c_state = self.IDLE
elif self.c_state == self.HEADER_M: elif self.c_state == self.HEADER_M:
if c == '>': if cc == '>':
self.c_state = self.HEADER_ARROW self.c_state = self.HEADER_ARROW
elif c == '!': elif cc == '!':
self.c_state = self.HEADER_ERR self.c_state = self.HEADER_ERR
else: else:
self.c_state = self.IDLE self.c_state = self.IDLE
@ -432,34 +422,33 @@ class PyMSP:
elif self.c_state == self.HEADER_ARROW or self.c_state == self.HEADER_ERR: elif self.c_state == self.HEADER_ARROW or self.c_state == self.HEADER_ERR:
self.err_rcvd = (self.c_state == self.HEADER_ERR) self.err_rcvd = (self.c_state == self.HEADER_ERR)
#print (struct.unpack('<B',c)[0]) #print (struct.unpack('<B',c)[0])
self.dataSize = ((struct.unpack('<B',c)[0])&0xFF) self.dataSize = ci
# reset index variables # reset index variables
self.p = 0 self.p = 0
self.offset = 0 self.offset = 0
self.checksum = 0 self.checksum = 0
self.checksum ^= ((struct.unpack('<B',c)[0])&0xFF) self.checksum ^= ci
# the command is to follow # the command is to follow
self.c_state = self.HEADER_SIZE self.c_state = self.HEADER_SIZE
elif self.c_state == self.HEADER_SIZE: elif self.c_state == self.HEADER_SIZE:
#print (struct.unpack('<B',c)[0]) #print (struct.unpack('<B',c)[0])
self.cmd = ((struct.unpack('<B',c)[0])&0xFF) self.cmd = ci
self.checksum ^= ((struct.unpack('<B',c)[0])&0xFF) self.checksum ^= ci
self.c_state = self.HEADER_CMD self.c_state = self.HEADER_CMD
elif self.c_state == self.HEADER_CMD and self.offset < self.dataSize: elif self.c_state == self.HEADER_CMD and self.offset < self.dataSize:
#print (struct.unpack('<B',c)[0]) #print (struct.unpack('<B',c)[0])
self.checksum ^= ((struct.unpack('<B',c)[0])&0xFF) self.checksum ^= ci
self.inBuf[self.offset] = c self.inBuf[self.offset] = ci
self.offset += 1 self.offset += 1
elif self.c_state == self.HEADER_CMD and self.offset >= self.dataSize: elif self.c_state == self.HEADER_CMD and self.offset >= self.dataSize:
# compare calculated and transferred checksum # compare calculated and transferred checksum
#print "Final step..." if ((self.checksum&0xFF) == ci):
if ((self.checksum&0xFF) == ((struct.unpack('<B',c)[0])&0xFF)):
if self.err_rcvd: if self.err_rcvd:
print "Copter didn't understand the request type" print("Vehicle didn't understand the request type")
else: else:
self.evaluateCommand(self.cmd, self.dataSize) self.evaluateCommand(self.cmd, self.dataSize)
else: else:
print '"invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF))' print('"invalid checksum for command "+((int)(cmd&0xFF))+": "+(checksum&0xFF)+" expected, got "+(int)(c&0xFF))')
self.c_state = self.IDLE self.c_state = self.IDLE
@ -490,8 +479,6 @@ class PyMSP:
payload.append(self.byteP[i]) payload.append(self.byteP[i])
payload.append(self.byteI[i]) payload.append(self.byteI[i])
payload.append(self.byteD[i]) payload.append(self.byteD[i])
print "Payload:..."
print payload
self.sendRequestMSP(self.requestMSP(self.MSP_SET_PID, payload, True), True) self.sendRequestMSP(self.requestMSP(self.MSP_SET_PID, payload, True), True)
@ -534,7 +521,7 @@ class PyMSP:
self.msp_raw_imu['gyry'] -= 65536 self.msp_raw_imu['gyry'] -= 65536
if self.msp_raw_imu['gyrz'] > 32768: if self.msp_raw_imu['gyrz'] > 32768:
self.msp_raw_imu['gyrz'] -= 65536 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']) 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) time.sleep(0.04)
timer = timer + (time.time() - start) timer = timer + (time.time() - start)
start = time.time() start = time.time()

Write Preview
Loading…
Cancel
Save