mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 14:13:42 -04:00
89802ed6fc
This becomes apparent when pack voltage is above DJI's hard limit of 25.5v with this fix the cell voltage is correct even for 12s packs just like on real hardware
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', "BHBHhBh", "cellCount,capacity,voltage,mah,current,state,voltage_cv"),
|
|
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)
|