Tuesday, January 25, 2011

Reading navigation data directly from Navboard

On A.R. Drones navigation board, the following sensors exist:

  • IDG-500 (Dual-axis gyroscope)
  • XV-3500CB (Z-axis gyroscope)
  • BMA150 (3-axis accelerometer)
  • Some ultrasonic stuff

For handling these sensors, a PIC24 is used. This device does the A/D conversion of the gyroscope data and the I2C communication with the BMA150.
Periodically, the sensor data is sent over UART to the mainboard where it is used by program.elf to control the Drone.

So... how to get this data without using program.elf?

First, start the sensor acquisition by calling:

echo -e -n "\1" > /dev/ttyPA2

Now, periodically, the nav boards sends a 0x2E sized frame to UART2.. so read it with:

dd if=/dev/ttyPA2 count=1 bs=46 | hexdump -C

The data seems to have the following format:

struct nav_data_tag
  u16 size;                /* +0x00 Size of the following data (0x2C) */
  u16 sequence;            /* +0x02 Sequence number */
  u16 raw_accs[3];         /* +0x04 Raw data of the accelerometers*/
  u16 raw_gyros[3];        /* +0x0A Raw data for the gyros */
  u16 raw_gyros_110[2];    /* +0x10 4.5x Raw data (IDG) */
  u16 uk_0x14;             /* +0x14 Unkown. Maybe accs temperature */
  u16 gyro_temp;           /* +0x16 Gyro temperature (IDG) */
  u16 vrefEpson;           /* +0x18 Gyro v_ref (Epson) */
  u16 vrefIDG;             /* +0x1A Gyro v_ref (IDG) */
  u16 uk_0x1C;             /* Unkown */
  u16 checksum;            /* +0x1E Checksum */
  u16 us_debut_echo;       /* +0x20 Ultrasonic parameter */
  u16 us_fin_echo;         /* +0x22 Ultrasonic parameter */
  u16 us_association_echo; /* +0x24 Ultrasonic parameter */
  u16 us_distance_echo;    /* +0x26 Ultrasonic parameter */
  u16 us_courbe_temps;     /* +0x28 Ultrasonic parameter */
  u16 us_courbe_valeur;    /* +0x2A Ultrasonic parameter */
  u16 us_courbe_ref;       /* +0x2C Ultrasonic parameter */ 

I haven't figured out much yet, but here are my first analysis results:

  • size: always 0x2C
  • sequence: increases every update
  • raw_accs: seems to contain the BMA values left-shifted by 2
  • raw_gyros: 12-bit A/D converted voltage of the gyros
  • raw_gyros_110: gyro values wieth another resolution (see IDG-500 datasheet)
  • gyro_temp: 12-bit A/D converted voltage of IDG's temperature sensor
  • vrefEpson: 12-bit A/D converted reference voltage of the Epson sensor
  • vrefIDG: as vrefEpson, but for IDG sensor
  • checksum: checksum (have to figure out how to calculate..
  • all others haven't yet figured out what they mean

Wednesday, January 19, 2011

Creating, testing (and flashing -- TBD) a custom kernel

In my last post, I announced that creating and uploading a custom kernel works. Now I will explain how this can be done.

Thanks to MAPGPS from http://www.ardrone-flyers.com for testing my tools.

You need:

- Linux! I used Ubuntu 10.10
- ARDrone linux kernel source (get it from [1])
- ARDrone linux kernel config (get it from [2])
- The Sourcery G++ toolchain (get it from [3] or [4] )
- My plftool, usb_flash tool (see below)
- The old parrot flash tool (for ardrone_usb_bootloader.bin)
- libusb-0.1 (linux) or libusb-win32 (windows)
- zlib

Getting & Compiling plftool + usb_flash tool

These tools are not yet released. Thus I will not provide binaries for windows, instead you have to compile them by yourself. For windows, you need to use mingw32.

Getting the sources

The sources are stored at my google code project. In order to get them, you need to install a svn client. For linux you then call something like this:

svn checkout http://ardrone-tool.googlecode.com/svn/projects/libplf/trunk libplf
svn checkout http://ardrone-tool.googlecode.com/svn/projects/usb_flash/trunk usb_flash
svn checkout http://ardrone-tool.googlecode.com/svn/projects/plftool/trunk plftool


Now that you got the sources, you need to compile them. First start with libplf as this is needed by all projects. The library contains the code to read and write .plf files.

cd libplf 
cd ..

When compiling with mingw32 call "make -f Makefile.w32" instead.

If libplf was compiled succesfully you should get either a libplf.so or libplf.dll file.
Next compile plftool and usb_flash:

cd plftool
cd ..
cd usb_flash
cd ..

When compiling with mingw32, call "make -f Makefile.w32" instead.

This should create plftool(.exe) and usb_flash(.exe).

For Linux, if you do not want to install the libplf.so file, you need to call plftool and usb_flash with LD_LIBRARY_PATH=<path_to_the_folder_where_libplf.so_is_located>  prefix.

LD_LIBRARY_PATH=/home/scorp2kk/projects/ardrone/tools/libplf plftool 

For Windows, you need to add the folder where libplf.dll is located to your PATH variable before calling usb_flash or plftool.

Patching the kernel

As Parrot's published source code does not match the kernel delivered with firmware > 1.3.3, a patch is required to ensure correct behaviour. This patch mainly affects p6_sdhci.c.

You can get the patch from here. To apply it, just call:

cd linux-2.6.27
patch -p1 < p6_sdhci-patch1.patch

Compiling the kernel

Now as the kernel is patched, you should use Parrot's original config [2] and cross-compile the sources with Sourcery G++. Basically you need to do something like this (don't forget to add the Sourcery' bin directory to your PATH):

cp ARDrone_Version_20100809_1_2-busybox.config .config
make ARCH=arm CROSS_COMPILE=arm-none-linux-gnueabi- zImage 

After a few minutes (or hours) make informs you that the zImage file is available:

Kernel: arch/arm/boot/Image is ready
Kernel: arch/arm/boot/zImage is ready

Creating a kernel.plf file

If you got your zImage, it is time to build a kernel.plf file from it. This is done by plftool. plftool expects a build-ini file that describes the plf file. A sample ini file can be found at plftool/samples/kernel. Using the sample kernel.ini, you only have to modify the "File=" parameters in section "[zImage]" and "[BootParams]" so they point to the zImage file and to a text file, the boot parameters are located in (e.g. called bootparams.txt).

To create the kernel.plf from the kernel.ini, just call:

plftool -b kernel.ini -o kernel.plf 

The tool should output something similar to this and create the kernel.plf file.

Creating kernel.plf based on this config:
File [HdrVersion: 11 EntryPoint: 0x40800000; Version: 0.0.0
TargetPlat=4, TargetAppl=78, HwCompat=0, LangZone=0]
  zImage     @ 0x40800000 (zImage)
  initrd     @ ---------- (no initrd)
  bootparams @ 0x40700000 (bootparams.txt)

 *** kernel.plf created ***

Testing the kernel.plf file

If  kernel.plf file was created, it is time to test this file. First inspect it with plftool:

plftool -d -i kernel.plf

Have a look at dwFileType, dwEntryPoint and Section->LoadAddr.

If it looks okay, it is time to start (not flash!) the kernel:
  1. Copy ardrone_usb_bootloader.bin to the folder of usb_flash
  2. Connect the drone to your PC via USB (the LED should be RED)
  3. Connect the BATTERY to your drone (the LED becomes GREEN) 
  4. Check if the device was found (with lsusb)
  5. Start usb_flash in TEST mode:
usb_flash -t kernel.plf

This will load (NOT flash) the kernel over USB. In general, it is helpful to observe A.R. Drone's serial port, as the whole boot sequence is shown there. usb_flash outputs on success:


Mode: kernel test

Verifying ardrone_usb_bootloader.bin
Verifying test_kernel.plf

Try [00/10] to connect to VID: 0x19cf PID: 0x1000
Found a possible device:
- Manufacturer: Parrot SA
- Product: P6 USB Stage1
- Serialnumber: ?
- Number of configurations: 1
Sending bootloader (0x5b20 bytes)
- Send Hello P6 (0xA3)

Checksum returned: 0x5256 Expected: 0x5256 => OK
Starting the bootloader...

Closing usb connection...
Try [00/10] to connect to VID: 0x19cf PID: 0x1000
Found a possible device:
- Manufacturer: Parrot SA
- Product: P6 USB Stage1
- Serialnumber: ?
- Number of configurations: 1
loading installer
Uploading file: test_kernel.plf
Error Code returned: 0x00000000  ==> OK

If everything works, the kernel boots up, WiFi ist started and you should be able to connect to the drone with telnet.

Check the kernel version with

uname -a

If the default kernel starts without problems, you can now begin to modify the kernel config.

Flashing the kernel to NAND works but I still need to modify the plftool.. so stay tuned.

Happy hacking :-)

[1] https://projects.ardrone.org/documents/show/19
[2] https://projects.ardrone.org/documents/show/18
[3] https://projects.ardrone.org/documents/show/20
[4] http://www.codesourcery.com/sgpp/lite/arm/portal/release858

Tuesday, January 11, 2011

Creating + Flashing Custom Kernel


Creating a kernel.plf file from zImage+initrd --> WORKS!
Booting this kernel.plf file over USB --> WORKS!
Flashing this kernel.plf to NAND --> to be done...

More will follow soon :-)

Sunday, January 9, 2011

Tools for A.R. Drone

I finally released some tools for A.R. Drone:
  1. libplf - A library to read / write plf files
  2. plf_inst_extract - A tool to extract and patch a ardrone_installer.plf from a given ardrone_update.plf
  3. usb_flash - A tool to flash a ardrone_update.plf over USB. This tool requires:
    1. ardrone_update.plf - the firmware to flash
    2. ardrone_installer.plf - can be obtained from ardrone_update.plf with plf_inst_extract
    3. ardrone_usb_bootloader.bin - was delivered with the old flash tool. I will probably write an open-source boot loader sooner or later.
Have a look at my goolge code project:


A.R. Drone's boot sequence

In order to be able to flash a custom kernel to A.R. Drone, it is necessary to understand it's boot sequence:
  1. After reset, the program counter starts at the beginning P6 internal ROM. In this internal ROM, some kind of pre-bootloader exists that evaluates external pins and selects the corresponding boot mode (e.g. boot over USB, NAND, IIC, UART, ... ). Depending on the selected mode, the peripherals are set-up and it is tried to boot over this peripheral.
    • Boot over NAND: The pre-bootloader starts the memory controller, copies the bootloader from NAND to internal RAM and sets the program counter to the address in internal RAM which basically starts the bootloader.
    • Boot over USB: The pre-bootloader listens to USB and waits for the "Hello P6" command. If received, it replies and awaits an image (the usb_bootloader.bin). The received data is copied to internal RAM and the program counter is changed to the internal RAM address wich in effect starts the bootloader.

  2. Depending on the started bootloader, either the UBI partitions are "mounted" and the kernel image is read or the bootloader waits until the kernel image is sent over USB.
  3. If the installer.plf (which is basically a kernel image) is booted over USB, the "init" command of this image awaits the actual firmware (ardrone_update.plf) over USB and installs the content to NAND.