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

AP_MSP: added python MSP FPV viewer

c415-sdk
Andrew Tridgell 5 years ago
parent
7c579c4814
commit
449883c7f1
2 changed files with 752 additions and 0 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. 148
      libraries/AP_MSP/Tools/msposd.py
  2. 604
      libraries/AP_MSP/Tools/pymsp.py

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

@ -0,0 +1,148 @@
#!/usr/bin/env python
'''
A tool to view MSP telemetry, simulating a DJI FPV display
This is used for testing changes to the ArduPilot implementation of MSP telemetry for FPV
Originally started by Alex Apostoli
based on https://github.com/hkm95/python-multiwii
'''
import pygame
import threading
import time
import sys
import pymsp
import argparse
import socket
import serial
import errno
parser = argparse.ArgumentParser(description='ArduPilot MSP FPV viewer')
parser.add_argument('--port', default="tcp:localhost:5763", help="port to listen on, can be serial port or tcp:IP:port")
args = parser.parse_args()
def connect(port):
'''connect to port, returning a receive function'''
try:
if port.startswith("tcp:"):
print("Connecting to TCP socket %s" % port)
a = port[4:].split(':')
port = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
port.connect((a[0],int(a[1])))
port.setblocking(0)
return port.recv
print("Connecting to serial port %s" % port)
port = serial.Serial(args.port, 115200, timeout=0.01)
return port.read
except Exception as ex:
return None
recv_func = None
last_read_s = time.time()
msp = pymsp.PyMSP()
pygame.init()
# define the RGB value for white,
# green, blue colour .
white = (255, 255, 255)
green = (0, 255, 0)
blue = (0, 0, 128)
black = (0, 0 ,0)
# window size
FontWidth = 25
FontHeight = 25
WindowWidth = 32 * FontWidth
WindowHeight = 16 * FontHeight
# create the display surface object
# of specific dimension..e(X, Y).
display_surface = pygame.display.set_mode((WindowWidth,WindowHeight))
# set the pygame window name
pygame.display.set_caption('MSP Display')
def item_to_pos(item):
'''map MSP item to a X,Y tuple or None'''
if item >= msp.msp_osd_config['osd_item_count']:
return None
pos = msp.msp_osd_config['osd_items'][item]
if pos < 2048:
return None
pos_y = (pos-2048) // 32
pos_x = (pos-2048) % 32
return (pos_x, pos_y)
def display_text(item, message):
'''display a string message for an item'''
XY = item_to_pos(item)
if XY is None:
return
(X,Y) = XY
text = font.render(message, True, white, black)
textRect = text.get_rect()
slen = len(message)
px = X * FontWidth
py = Y * FontHeight
textRect.center = (px+textRect.width/2, py+textRect.height/2)
display_surface.blit(text, textRect)
def display_all():
'''display all items'''
display_text(msp.OSD_CURRENT_DRAW, "%.2fA" % (msp.msp_battery_state['current']*0.001))
display_text(msp.OSD_MAIN_BATT_VOLTAGE, "%.2fV" % (msp.msp_battery_state['voltage']*0.001))
display_text(msp.OSD_GPS_SPEED, "%.1fm/s" % (msp.msp_raw_gps['GPS_speed']*0.01))
display_text(msp.OSD_GPS_SATS, "%uS" % (msp.msp_raw_gps['GPS_numSat']))
display_text(msp.OSD_ALTITUDE, "%.1fm" % (msp.msp_altitude['alt']*0.01))
display_text(msp.OSD_ROLL_ANGLE, "Roll:%.1f" % (msp.msp_attitude['roll']*0.1))
display_text(msp.OSD_PITCH_ANGLE, "Pitch:%.1f" % (msp.msp_attitude['pitch']*0.1))
display_text(msp.OSD_CRAFT_NAME, "%s" % (msp.msp_name['name']))
def receive_data():
'''receive any data from socket'''
global recv_func, last_read_s
while recv_func is None:
time.sleep(0.5)
recv_func = connect(args.port)
try:
buf = recv_func(100)
if not buf:
if time.time() - last_read_s > 1:
recv_func = None
return
last_read_s = time.time()
except IOError as e:
if e.errno == errno.EWOULDBLOCK:
return
recv_func = None
return
msp.parseMspData(buf)
font = pygame.font.Font('freesansbold.ttf', 12)
last_display_t = time.time()
# infinite loop
while True:
receive_data()
now = time.time()
if now - last_display_t > 0.1:
# display at 10Hz
last_display_t = now
display_surface.fill(black)
display_all()
pygame.display.update()
time.sleep(0.01)
for event in pygame.event.get():
if event.type == pygame.QUIT :
pygame.quit()
quit()

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

@ -0,0 +1,604 @@
#!/usr/bin/env python
"""
author: Alex Apostoli
based on https://github.com/hkm95/python-multiwii
which is under GPLv3
"""
import struct
import time
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_SERVO_CONF =120
MSP_BATTERY_STATE =130
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_SELECT_SETTING =210
MSP_SET_HEAD =211
MSP_SET_SERVO_CONF =212
MSP_SET_MOTOR =214
MSP_BIND =241
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
def __init__(self):
self.msp_name = {
'name':None
}
self.msp_ident = {
'version':None,
'multiType':None,
'multiCapability':None
}
self.msp_status = {
'cycleTime':None,
'i2cError':None,
'present':None,
'mode':None
}
self.msp_raw_imu = {
'size':0,
'accx':0.0,
'accy':0.0,
'accz':0.0,
'gyrx':0.0,
'gyry':0.0,
'gyrz':0.0
}
self.msp_set_rc = {
'roll':0,
'pitch':0,
'yaw':0,
'throttle':0,
'aux1':0,
'aux2':0,
'aux3':0,
'aux4':0
}
self.msp_raw_gps = {
'GPS_fix':0,
'GPS_numSat':0,
'GPS_latitude':0,
'GPS_longitude':0,
'GPS_altitude':0,
'GPS_speed':0
}
self.msp_comp_gps = {
'GPS_distanceToHome':0,
'GPS_directionToHome':0,
'GPS_update':0
}
self.msp_attitude = {
'roll':0,
'pitch':0,
'yaw':0
}
self.msp_altitude = {
'alt':0,
'vspeed':0
}
self.msp_rc_tuning = {
'byteRC_RATE':0,
'byteRC_EXPO':0,
'byteRollPitchRate':0,
'byteYawRate':0,
'byteDynThrPID':0,
'byteThrottle_MID':0,
'byteThrottle_EXPO':0
}
self.msp_misc = {
'intPowerTrigger': 0
}
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.msp_battery_state = {
'cellCount':0,
'capacity':0,
'voltage':0,
'mah':0,
'current':0
}
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 = []
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", str(self.inBuf[self.p:self.p+2]))
self.p += 2
return value
def read16u(self):
'''unsigned 16 bit number'''
value, = struct.unpack("<H", str(self.inBuf[self.p:self.p+2]))
self.p += 2
return value
def read8(self):
'''unsigned 8 bit number'''
value, = struct.unpack("<B", str(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)
#print "here in requesrMSP"
#print bf
return bf
def evaluateCommand(self, cmd, dataSize):
if cmd == self.MSP_NAME:
s = ''
for i in range(0,dataSize,1):
b = self.read8()
if b == 0:
break
s += chr(b)
self.msp_name['name'] = s
elif cmd == self.MSP_IDENT:
self.msp_ident['version'] = self.read8()
self.msp_ident['multiType'] = self.read8()
self.read8() # MSP version
self.msp_ident['multiCapability'] = self.read32u()
elif cmd == self.MSP_STATUS:
self.msp_status['cycleTime'] = self.read16u()
self.msp_status['i2cError'] = self.read16u()
self.msp_status['present'] = self.read16u()
self.msp_status['mode'] = self.read32u()
elif cmd == self.MSP_RAW_IMU:
self.msp_raw_imu['accx'] = float(self.read16())
self.msp_raw_imu['accy'] = float(self.read16())
self.msp_raw_imu['accz'] = float(self.read16())
self.msp_raw_imu['gyrx'] = float(self.read16())
self.msp_raw_imu['gyry'] = float(self.read16())
self.msp_raw_imu['gyrz'] = float(self.read16())
self.msp_raw_imu['magx'] = float(self.read16())
self.msp_raw_imu['magy'] = float(self.read16())
self.msp_raw_imu['magz'] = float(self.read16())
self.msp_raw_imu['size'] = dataSize
elif cmd == self.MSP_SERVO:
for i in range(0,8,1):
self.servo.append(self.read16())
elif cmd == self.MSP_MOTOR:
for i in range(0, 8, 1):
self.mot.append(self.read16())
elif cmd == self.MSP_RC:
for i in range(0, 8, 1):
self.RCChan.append(self.read16())
elif cmd == self.MSP_RAW_GPS:
self.msp_raw_gps['GPS_fix'] = self.read8()
self.msp_raw_gps['GPS_numSat'] = self.read8()
self.msp_raw_gps['GPS_latitude'] = self.read32()
self.msp_raw_gps['GPS_longitude'] = self.read32()
self.msp_raw_gps['GPS_altitude'] = self.read16()
self.msp_raw_gps['GPS_speed'] = self.read16()
elif cmd == self.MSP_COMP_GPS:
self.msp_comp_gps['GPS_distanceToHome'] = self.read16()
self.msp_comp_gps['GPS_directionToHome'] = self.read16()
self.msp_comp_gps['GPS_update'] = self.read8()
elif cmd == self.MSP_ATTITUDE:
self.msp_attitude['roll'] = self.read16()
self.msp_attitude['pitch'] = self.read16()
self.msp_attitude['yaw'] = self.read16()
elif cmd == self.MSP_ALTITUDE:
self.msp_altitude['alt'] = self.read32()
self.msp_altitude['vspeed'] = self.read16()
elif cmd == self.MSP_ANALOG:
x = None
elif cmd == self.MSP_RC_TUNING:
self.msp_rc_tuning['byteRC_RATE'] = self.read8()
self.msp_rc_tuning['byteRC_EXPO'] = self.read8()
self.msp_rc_tuning['byteRollPitchRate'] = self.read8()
self.msp_rc_tuning['byteYawRate'] = self.read8()
self.msp_rc_tuning['byteDynThrPID'] = self.read8()
self.msp_rc_tuning['byteThrottle_MID'] = self.read8()
self.msp_rc_tuning['byteThrottle_EXPO'] = self.read8()
elif cmd == self.MSP_ACC_CALIBRATION:
x = None
elif cmd == self.MSP_MAG_CALIBRATION:
x = None
elif cmd == self.MSP_PID:
for i in range(0, 8, 1):
self.byteP[i] = (self.read8())
self.byteI[i] = (self.read8())
self.byteD[i] = (self.read8())
if (i != 4) and (i != 5) and (i != 6):
self.confP[i] = (float(self.byteP[i])/10.0)
self.confI[i] = (float(self.byteI[i])/1000.0)
self.confD[i] = (float(self.byteD[i]))
self.confP[4] = (float(self.byteP[4]) / 100.0)
self.confI[4] = (float(self.byteI[4]) / 100.0)
self.confD[4] = (float(self.byteD[4]) / 1000.0)
self.confP[5] = (float(self.byteP[5]) / 10.0)
self.confI[5] = (float(self.byteI[5]) / 100.0)
self.confD[5] = (float(self.byteD[5]) / 1000.0)
self.confP[6] = (float(self.byteP[6]) / 10.0)
self.confI[6] = (float(self.byteI[6]) / 100.0)
self.confD[6] = (float(self.byteD[6]) / 1000.0)
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_MISC:
self.msp_misc['intPowerTrigger'] = self.read16u()
for i in range(0,4,1):
self.MConf[i] = self.read16u()
self.MConf[4] = self.read32u()
self.MConf[5] = self.read32u()
elif cmd == self.MSP_MOTOR_PINS:
for i in range(0, 8, 1):
self.byteMP.append(self.read16())
elif cmd == self.MSP_DEBUGMSG:
x = None
elif cmd == self.MSP_DEBUG:
x = None
elif cmd == self.MSP_OSD_CONFIG:
self.msp_osd_config['feature'] = int(self.read8()) # 8
self.msp_osd_config['video_system'] = self.read8() # 8
self.msp_osd_config['units'] = self.read8() # 8
self.msp_osd_config['rssi_alarm'] = self.read8() # 8
self.msp_osd_config['cap_alarm'] = self.read16() # 16
self.msp_osd_config['unusaed_1'] = self.read8() # 8
self.msp_osd_config['osd_item_count'] = self.read8() # 8
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'][i] = self.read16u() # x 16
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'][i] = self.read16u() # x 16
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'][i] = self.read16u() # x 16
self.msp_osd_config['legacy_warnings'] = self.read16u() # 16
self.msp_osd_config['warnings_count'] = self.read8() # 8
self.msp_osd_config['enabled_warnings'] = self.read32u() # 32
self.msp_osd_config['profiles'] = self.read8() # 8
self.msp_osd_config['selected_profile'] = self.read8() # 8
self.msp_osd_config['osd_overlay'] = self.read8()
# 8
elif cmd == self.MSP_BATTERY_STATE:
self.msp_battery_state['cellCount'] = self.read8()
self.msp_battery_state['capacity'] = self.read16u()
self.msp_battery_state['voltage'] = self.read8()
self.msp_battery_state['mah'] = self.read16u()
self.msp_battery_state['current'] = self.read16()
def parseMspData(self, buf):
for c in buf:
self.parseMspByte(c)
def parseMspByte(self, c):
if self.c_state == self.IDLE:
if c == '$':
self.c_state = self.HEADER_START
else:
self.c_state = self.IDLE
elif self.c_state == self.HEADER_START:
if c == 'M':
self.c_state = self.HEADER_M
else:
self.c_state = self.IDLE
elif self.c_state == self.HEADER_M:
if c == '>':
self.c_state = self.HEADER_ARROW
elif c == '!':
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 = ((struct.unpack('<B',c)[0])&0xFF)
# reset index variables
self.p = 0
self.offset = 0
self.checksum = 0
self.checksum ^= ((struct.unpack('<B',c)[0])&0xFF)
# 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 = ((struct.unpack('<B',c)[0])&0xFF)
self.checksum ^= ((struct.unpack('<B',c)[0])&0xFF)
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 ^= ((struct.unpack('<B',c)[0])&0xFF)
self.inBuf[self.offset] = c
self.offset += 1
elif self.c_state == self.HEADER_CMD and self.offset >= self.dataSize:
# compare calculated and transferred checksum
#print "Final step..."
if ((self.checksum&0xFF) == ((struct.unpack('<B',c)[0])&0xFF)):
if self.err_rcvd:
print "Copter 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])
print "Payload:..."
print payload
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)
Write Preview
Loading…
Cancel
Save