def _receive(self, packet):
'''One packet received by the ReceiverThread.
This immediately handles some packets,
and queues responses to be picked up later by _waitResponse().
'''
if packet.framing == fyproto.LONG_FORM:
if packet.command == 0x00:
self.cmd00 = packet
_unknown, version = struct.unpack("<HH", packet.data)
self.version = version / 100.0
return
if packet.framing == fyproto.SHORT_FORM:
if packet.command == 0x0B:
if self.verbose:
print("Connecting to gimbal, firmware version %s" %
self.version)
with self.connectedCV:
self.connected = True
self.send(
fyproto.Packet(target=0,
command=0x0b,
data=bytes([0x01])))
self.connectedCV.notify_all()
return
if packet.target == 0x03:
self.responseQueue.put(packet)
return
def nextMicrocontroller():
send(
fyproto.Packet(command=0x07,
target=0,
framing=fyproto.LONG_FORM,
data=b'\x01'))
return waitResponse(0x08)
def saveParams(self, targets=axes, timeout=None, retries=None):
for target in targets:
p = fyproto.Packet(target=target, command=0x05, data=b'\x00')
r = self.transaction(p, timeout=timeout, retries=retries)
if struct.unpack('<B', r.data)[0] != target:
raise IOError("Failed to save parameters, response %r" %
packet)
if self.verbose:
print("Saved params on MCU %d" % target)
def setMotors(self, enable, targets=axes):
# Not sure if order matters
for t in sorted(targets, reverse=True):
self.send(
fyproto.Packet(target=t,
command=0x03,
data=struct.pack('B', enable)))
if enable:
# Unknown
self.setParam(target=2, number=0x67, value=1)
def writeBlock(number, content):
header = struct.pack("<HH", number, 0)
send(
fyproto.Packet(command=0x02,
target=0,
framing=fyproto.LONG_FORM,
data=header + content))
r = waitResponse(0x03)
if len(r.data) != 2 or struct.unpack('<H', r.data)[0] != number:
raise ValueError(
"Unexpected response to firmware block 0x%04x write: %s" %
(number, r))
import fyproto
import serial
import argparse
import binascii
def hexint(x):
return int(x, 16)
parser = argparse.ArgumentParser(description='Talk to the Feiyu Tech gimbal')
parser.add_argument('--port', default='/dev/ttyAMA0')
parser.add_argument('--framing', type=hexint, default=fyproto.SHORT_FORM)
parser.add_argument('--command', type=hexint, default=None)
parser.add_argument('--target', type=hexint, default=0)
parser.add_argument('--read', action='store_true')
parser.add_argument('data', nargs='*')
args = parser.parse_args()
port = serial.Serial(args.port, baudrate=115200)
rx = fyproto.PacketReceiver()
if args.command is not None:
data = binascii.a2b_hex(''.join(args.data))
packet = fyproto.Packet(command=args.command, target=args.target, framing=args.framing, data=data)
print(packet)
port.write(packet.pack())
if args.read:
while True:
for packet in rx.parse(port.read(1)):
print(packet)
parser = argparse.ArgumentParser(description='Gimbal flasher')
parser.add_argument('--port', default='/dev/ttyAMA0')
parser.add_argument('--first-block', type=hexint, default=0)
parser.add_argument('--num-blocks', type=hexint, default=None)
parser.add_argument('filename')
args = parser.parse_args()
fw = FirmwarePackage(args.filename)
port = serial.Serial(args.port, baudrate=115200)
rx = fyproto.PacketReceiver()
# Power must be off before running this
print("Power on the gimbal now")
# Look for the bootloader's "hello" announcement
hello = waitResponse(0x00)
_unknown, version = struct.unpack("<HH", hello.data)
print("Connected to version %s" % (version / 100.0))
# Respond to the bootloader, prevent normal boot
send(
fyproto.Packet(command=0x01, target=0, framing=fyproto.LONG_FORM,
data=b''))
# Each microcontroler is programmed, then we ask it to become a pass-through for the next
for mcu in range(3):
for i in range(args.num_blocks or fw.sizes[mcu]):
blockNum = i + args.first_block
writeBlock(blockNum, fw.block(mcu, blockNum))
nextMicrocontroller()
def setParam(self, target, number, value, fmt='h'):
self.send(
fyproto.Packet(target=target,
command=0x08,
data=struct.pack('<BB' + fmt, number, 0, value)))
def getParam(self, target, number, fmt='h', timeout=None, retries=None):
p = fyproto.Packet(target=target,
command=0x06,
data=struct.pack('B', number))
r = self.transaction(p, timeout=timeout, retries=retries)
return struct.unpack('<' + fmt, r.data)[0]
def storeCalibrationAngle(self, num, targets=axes):
for target in targets:
p = fyproto.Packet(target=target,
command=0x0c,
data=struct.pack('B', num))
self.transaction(p)
def setFn(self, x, y, z, m):
self.controlPacket = fyproto.Packet(target=0,
command=0x01,
data=struct.pack(
'<hhhB', x, y, z, m))
print(self.controlPacket)
