Bluetooth Motor Control on Kria KR260

One of my favorite and most popular projects was my Lego Land Rover Defender robot that I did last year. If you watched the videos I did to document that build process or read its very lengthy project write-up, then you're probably aware of the laundry list of upgrades I had come up with throughout the build. One of the main upgrades I wanted to make was the motor system. The original motor system wasn't optimal for the structure of the Lego build and unfortunately caused the whole chassis to flex in unexpected ways during operation. This lead to the belt between the main motor and drive shaft to slip randomly. And with this motor system upgrade, I found a new method for controlling motors wirelessly from my Kria KR260 via Bluetooth.

Background

I feel like the Amazon algorithm had to have known what I was doing in my first build and intentionally hid this motor conversion kit for the 42110 Lego Technic Land Rover Defender from me as a cruel joke (I know, I know, it's my own fault for missing it, but I'm blaming the Amazon algorithm gods because it makes me feel better haha). But alas, I kid you not, the morning after I completed the first Lego Defender build, I woke up to the below item at the top of my "Recommended for you" list:

This motor conversion kit was curated/deisgned specifically for the Lego Technic 42110 Land Rover Defender. It came with the necessary spare parts and alternative build instructions as they deviated from the Lego-provided build instructions. The motor controller box (top right in photo above) is equipped with a bluetooth module that receives commands to drive/steer the Defender from either a remote control also provided in the kit, or an iOS/Android app on your phone.

The fact that it came with a smart phone app gave me the idea of running a bluetooth trace logger on my phone as I was using the app to capture the various command packets being sent to the motor controller module in the Lego Defender so I could re-create them from the KR260. Therefore giving me wireless control over the driving/steering of the Lego Defender via Bluetooth.

Bluetooth Sniffing

This idea of sniffing Bluetooth packets from a smart phone during the operation of an app is a standard way developers test/debug their apps, so there are plenty of tools available. Since I have an iPhone and Mac laptop, I dug into the Apple Developer suite of tools in Xcode. I found Apple's bluetooth sniffer application, PacketLogger, under Additional Tools for Xcode in Apple's Developer site. I then followed the instructions for debugging iOS bluetooth apps I found here on the official Bluetooth website.

I installed PacketLogger from Additional Tools for Xcode on my MacBook and opened a new iOS trace window under File > New iOS Trace:

I then downloaded a bluetooth profile from the Apple Developer website to log all bluetooth activity on my phone, installed it as prompted, and rebooted my phone.

After my phone rebooted, I plugged it into my computer and hit start in PacketLogger.

Then I opened the app for the motor controller on my phone and connected to the Lego Defender.

Once connected to the Lego Defender, I sent all possible commands multiple times in an obvious pattern so I could pick through what was what later.

I saved the trace file which I found could also be opened in Wireshark for analysis, but the format in PacketLogger wasn't much different and I ended up copy+pasting the hex data packets into a spreadsheet anyways so I could group the data in different ways in an attempt to spot patterns better.

Reverse Engineering Bluetooth Packets

First things first, I was able to see that the bluetooth address of the motor controller box was 45:71:21:BD:26:13. Then I was pretty quickly able to notice that I only needed the packets with the UUID 0xfff2, as they were the only packets transmitted during times I was actually seeing the Lego Defender do what I was telling it to from the app.

And of those packets, only the last 6 words (1 word = 2 bytes) changed based on whether I drove the Defender forward, backward, left, right, or stopped it. The rest of the services just seemed to be extra features with the iOS app that I didn't need to recreate for my purposes. So this narrowed things down to a much more manageable amount of data.

Of those last six words, I found that word 1 (B1) was always 0x01. The motor control app in iOS has a setting that allows it to connect to up to three different motor controller modules at once. Since I had my Defender's motor control box connected as module one in the app, that told me that word 1 in the bluetooth packet was this module ID and therefore could always be set to 0x01 in my use case.

I then noticed that word 5 (B5) was always set to 0x01 when I drove the Defender forward, and always set to 0x02 when I drove the Defender in reverse. Likewise, word 3 (B3) was always set to 0x04 when I steered the Defender right, and always set to 0x08 when I steered the Defender left.

Now I initially had assumed that the last word, word zero (B0), was a CRC. But after spending too much time trying various CRC lengths in a CRC calculator to make sure I had tried every combination possible, I came to the eventual conclusion that there was no CRC being used in the packets and I didn't know what word zero was.

I eventually noticed a pattern that lead me to discover that a check sum of the target motor channel and accompanying value was actually what the last two bytes of the packet were. Word zero (B0) was always 0x33 minus the channel direction, subtracted from the passed channel value overall. So:

Drive motors = Word 5 (B5): 0x01 for forward, 0x02 for reverse, and the checksum would be: B0 = B4 - (0x33 - B5)

Steer motor = Word 3 (B3): 0x04 for right, 0x08 for left, and the checksum would be: B0 = B2 - (0x33 - B3)

Note: all of the above values and calculations are in hexadecimal.

I'm not sure what the accompanying value is passed in words 2 and 4 (B2 & B4) as I couldn't see any noticeably different behavior regardless of what value I passed it. The only thing that seemed to matter was that I sent the right checksum with it. Whatever value I sent the target motor channel would cause it to turn at the same speed indefinitely until I sent the stop command.

My original guess had been that the value was either correlated with a duration or speed to drive the target motor channel at. However, I noticed that the second I let off the control buttons in the app that it would continuously transmit the stop command, so the value wasn't a duration to drive the motor for. I then tried very low values followed by high values to see if the motors turned at different speeds, but there was no noticeable difference.

Ultimately, I was able to determine the following format for the bluetooth packets for the 0xfff2 service:

It's also worth noting that the bluetooth motor controller is obviously not encrypting the packets, otherwise I wouldn't have been able to read the packets without the key for decrypting them.

Adding Bluetooth to KR260

I have to say that the developers at AMD and Canonical that created the Ubuntu 22.04 desktop image for the Kria boards did an outstanding job because I found that almost any USB Bluetooth dongle that works with Raspbian on a Raspberry Pi out-of-the-box will also work with the Ubuntu 22.04 desktop image on the Kria out-of-the-box with no extra setup. Gatttool is also pre-installed in the image as well. So actually getting Bluetooth working on my KR260 was the easiest part of this, I just had to insert the dongle into one of the USB ports of the KR260 and reboot the board.

Then I used gatttool to verify that the KR260 could actually connect to the bluetooth motor controller box. I launched interactive mode and connected to the its bluetooth address I got from PacketLogger using the connect command and viewed its available services using the primary and characteristics commands:

~$ sudo gatttool -I 
	[LE]> connect 45:71:21:BD:26:13
	[LE]> primary 
	[LE]> characteristics

I know from PacketLogger that the motor control commands are under the service with the 0xfff2 UUID. So the characteristics command tells me the full UUID that I need to use in my Python script is 0000fff2-0000-1000-8000-00805f9b34fb:

At this point, I know the packets I need to send to control the motors as I want and I've established the bluetooth connection between the KR260 and motor controller box in the Lego Defender. So the next step is to write the Python script to automate everything.

Python Test Script to Connect to Motor Controller

The only Python dependency that isn't pre-installed in the AMD Ubuntu 22.04 desktop image is the Bleak GATT client software. Bleak connects to BLE devices acting as GATT servers and provides an asynchronous, cross-platform Python API to connect and communicate with devices.

Install Bleak using pip:

ubuntu@kria:~$ sudo pip3 install bleak

Using the asyncio library to make the bluetooth operations nonblocking, I scripted up a version of the example code from the Bleak reference page:

import asyncio 
from bleak import BleakClient

bt_addr = "45:71:21:BD:26:13"
bt_uuid = "0000fff2-0000-1000-8000-00805f9b34fb"

async def main (bt_addr): 
    async with BleakClient(bt_addr) as client:
        header = bytes.fromhex("5A6B020005")
        portAB_command = bytes.fromhex("0000")
        portCD_command = bytes.fromhex("0000")
        module_channel = bytes.fromhex("01")
        command_chksum = bytes.fromhex("CD")
        model_number = await client.write_gatt_char(bt_uuid, header + portAB_command + portCD_command + module_channel + command_chksum) 


asyncio.run(main(bt_addr))

Then executed the script on the KR260:

ubuntu@kria:~$ python3 bt_motor_test.py

I'm sending the stop command to the motor controller in the above code, but I tested all of the possible combinations for driving forward/reverse and steering left/right and it worked great!