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

11 comments:

  1. Cool! I'm waiting for some free time so I can write custom code for flying the ARDrone as well!

    ReplyDelete
  2. Can we connect our hardware directly to navboard (without mother board) ? Have you pin description navboard/mother board ?

    ReplyDelete
  3. Hi,

    no, sorry. I do not have the complete pinout. But it shouldn't be too hard to figure out.

    There must be at least Rx,Tx,3.3V,GND and probably 5V.

    Best regards

    ReplyDelete
  4. Hi,

    There is Rx, Tx, 5V (no 3.3V I think), GND, /CS and 2 signals for ICSP. I tried to send the "\1" with /CS=0 without reaction on receive pin...
    Still interested for some info.

    My guess :

    1 : +Vcc (5v)
    2 : PGED2/RP10(1)/CN16/PMD2/RB10 (8) (ICSP)
    3 : Txd (to Navboard)
    4 : PGEC2/RP11(1)/CN15/PMD1/RB11 (9) (ICSP)
    5 : Rxd (from Navboard)
    6 :/MCLEAR (18)
    7 : GND
    8 : GND

    ReplyDelete
  5. Hi,

    Thanks, i think you are right as I measured the same yesterday.

    I can say for sure that 2 is PGED2 and 4 is PGEC2. Both are used together with MCLR for flashing the navboard F/W (called MYKONOS.HEX).

    MCLR is also toggled at startup of program.elf to reset the Navboard. Must be set 1 in normal operation.

    Vcc is +5V, but there is a voltage regulator on the navboard.

    Best regards

    ReplyDelete
  6. Hi, If you haven't figured this out yet, I think the "u16 uk_0x14;" part is actually the battery raw voltage in mv (vbat_raw). Check line 298 of the following

    https://projects.ardrone.org/embedded/ardrone-api/d9/d3d/navdata__common_8h-source.html#l00290

    ReplyDelete
  7. hummm, I think it's difficult for navigation board to measure vbat... not vbat on the connector...

    ReplyDelete
  8. HI,
    nice article! ;)
    I’ve got a question:
    This code set the RESET_pin of the navboard to high. But in this way, if the /dev/ttyPA2 is busy, is still possible to fly the drone?

    Thanks!

    ReplyDelete
  9. Hi,

    great work, I'm working on reading my navdata, but I got a size of 0x30, so the structure isn't complete.
    My navboard is a 6.0 hardware and 4.324 software.

    Where did you get these information so i can check for update.

    ReplyDelete
  10. Hello,

    Would you like to tell me how you find out this information? What kind of reverse engineering method did you use? I would like to find the same data for AR.Drone2 for an autonomous flight project. The project will be open source.

    ~ Dino

    ReplyDelete
  11. Oh,wow that was the much needed information,good work you have done dear..Please share some more thought of views about it..
    Skycaddie Golf

    ReplyDelete