New kernel source available

Nice. Seems like parrot uploaded a newer version of A.R.Drone's kernel source code.

See here:

https://projects.ardrone.org/documents/show/19

At least the p6_sdhci driver is correct now :-)

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
  

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

Compiling

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 
make
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
make
cd ..
cd usb_flash
make
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.

E.g.

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

*** VERIFICATION ***
Verifying ardrone_usb_bootloader.bin
Verifying test_kernel.plf


*** DOWNLOADING USB BOOTLOADER ***
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)
Success!!!

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

*** INSTALLER DOWNLOAD ***
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
*** INSTALLATION DONE ***

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

Creating + Flashing Custom Kernel

Yeah...

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 :-)

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:

http://code.google.com/p/ardrone-tool

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.

A.R. Drone USB (Updated)

A word of warning: You should not do anything like the following if you do not know what you do. You may ruin your drone.

As A.R. Drone has an USB OTG Port, it should be possible to use this port for extending the functionality of A.R. Drone.

But sadly Parrot disabled this port and only allows to use it for flashing purpose. Luckily they provide the source code for the driver [1].

So... let's have a closer look to this USB port..

First, Parrot defined the port as device only.

arch/arm/parrot6/mykonos.c, line 109:
static dwc_otg_info_t usb0_info = {
 .ctrl_mode = 2,
 .sof_filter = 7,
 .reset_pin = -1,
 .vbus_detection = 0,
 .fiq_enable = 0,
};

According to dwc_otg driver:

drivers/parrot/usb/dwc_otg/dwc_otg_cil.h
 /**
  * Controller mode
  * 0 - OTG
  * 1 - host only
  * 2 - device only
  */
 int32_t ctrl_mode;
#define DWC_OTG_HOST_DEVICE 0
#define DWC_OTG_HOST_ONLY   1
#define DWC_OTG_DEVICE_ONLY 2

In order to enable the USB host mode, it is required to overwrite/ignore the usb0_info defined in mykonos.c.

This can be achieved by patching dwc_otg_driver.c where the platform data is copied to the local parameter data. This is done by function "dwc_otg_set_specific_param". Also, by default (if not overwritten by platform data) the local parameters are configured as DWC_OTG_HOST_DEVICE. So simply not copying this parameter would prevent the driver from going to DEVICE_ONLY mode.

Thus the driver is modified and the assignment "params->ctrl_mode = info->ctrl_mode" is removed:

drivers/parrot/usb/dwc_otg/dwc_otg_driver.c, line 224:

/**
 * This function is called to set specific configuration parameters
 */
static void 
dwc_otg_set_specific_param
(
dwc_otg_info_t *info,
dwc_otg_core_params_t *params,
int port
)
{
 params->sof_filter = info->sof_filter;
 params->reset_pin = info->reset_pin;
 params->speed = info->speed;
 if (port == 0) {
  //params->ctrl_mode = info->ctrl_mode;
  //params->vbus_detection = info->vbus_detection;
 }
 if (port == 1 && !usb1_disable_fiq) {
  params->fiq_enable = info->fiq_enable;
 }
}

[Update]
As noted by MAPGPS [3], there is still an issue with the driver causing Oops. To fix this issue, it is necessary to change the overcurrent_pin from 89 to -1:

drivers/parrot/usb/dwc_otg/dwc_otg_driver.c, line 135:

.overcurrent_pin = -1, /* default */

[/Update]



After compiling, ftp-ing the module to the drone and calling

# insmod dwc_otg.ko

Oh no! dmesg shows that something gone wrong.. also VBus is still +0 V :-(

[   39.862534] dwc_otg: version 2.70a-parrot 22/03/2009
[   39.862703] dwc_otg dwc_otg.0: dwc_otg_driver_probe(c034d578)
[   39.862736] dwc_otg dwc_otg.0: start=0xc0400000
[   39.862851] dwc_otg dwc_otg.0: base=0xc8e00000
[   39.862883] dwc_otg dwc_otg.0: specific configuration
[   39.862914] dwc_otg dwc_otg.0: dwc_otg_device=0xc7a0b6e0
[   39.873574] DWC_otg: dwc_otg_core_reset() HANG! Soft Reset
 GRSTCTL=80000001
[   39.980233] DWC_otg: dwc_otg_core_reset() HANG! Soft Reset
 GRSTCTL=80000001

Looking at the drivers code, it seems like this HANG! message indicates a communication problem with the PHY. Checking the voltage levels of the PHY, it seems like almost all pins are tri-stated.

Having a look at the PCB, a SMSC USB3317 can be identified. According to its datasheet there is a pin named RESETB. When this pin is set to low the PHY is suspended and all I/O are tri-stated.

By monitoring RESETB it can be observed that RESETB is toggled by program.elf. So dumping all GPIO registers before and after starting program.elf the pin can be found.

And voilá: GPIO_127 is used to enable/disable the PHY. Program.elf sets it to output a high level which then switches a transistor and pulls RESETB to low.

In order to activate the USB as host GPIO_127 needs to be set to input level. This can be done either by ioctl or by the gpio command:

# gpio 127 -d i

After re-inserting dwc_otg.ko, dmesg no longer shows the HANG! message and VBUS has +5V :-)

Let's try to connect an USB device. The pinout for the USB connector is described in [2]. I use an old USB thumb drive (128MB) for testing.

dmesg shows:

[ 1675.054378] usb-storage: USB Mass Storage device detected
[ 1675.054650] usb-storage: -- associate_dev
[ 1675.054680] usb-storage: Vendor: 0x0ea0, Product: 0x2168,
 Revision: 0x0200
[ 1675.054707] usb-storage: Interface Subclass: 0x06, Protocol:
 0x50
[ 1675.054741] usb-storage: Transport: Bulk
[ 1675.054761] usb-storage: Protocol: Transparent SCSI
[ 1675.055439] scsi1 : SCSI emulation for USB Mass Storage devices
[...]
[ 1680.071311] scsi 1:0:0:0: Direct-Access              128MB       
     2.00 PQ: 0 ANSI: 2

Seems to work :-)


[1] https://projects.ardrone.org/documents/show/19
[2] https://projects.ardrone.org/attachments/167/ARDrone-USB-Cable.png
[3] http://www.ardrone-flyers.com/forum/viewtopic.php?p=5823#p5823