Trying out GPS with the Crazyflie

Before the holidays we said that we would be doing some testing with attaching a GPS receiver to the Crazyflie. For now it’s just a bit of a quick hack, but we are planning on doing more development. Here’s a quick summary of where we’re currently at (yes, that’s a bad joke..).

We attached a GPS module based on the u-blox MAX-7 chip which is interfaced using the UART. The initial plan was to interface it using I2C, but this will probably not work out. We thought that we could use the I2C interface for reading out the data via a normal memory map (like an EEPROM), but the module will continuously stream the data on the bus. This means that the module probably won’t play nice with other devices on the bus (which kind of defeats the purpose of the bus in the first place). So UART it is. By default the module sends NMEA data every second over the interface. There’s lots of information to get here, but what we focused on was the fix status of the module and latitude/longitude/altitude. Currently the firmware doesn’t contain any string library so parsing data from strings sent on the UART isn’t that easy. Instead we decided to just forward all the incoming data on the UART to the CRTP console. On the client side the NMEA data is picked up from the console and parsed. This data is then visualized using KDE Marble, where the position is shown on a map fetched from OpenStreetMap.

So what now? Well, there’s a few things more that we would like to do. First of all the data shouldn’t be sent over the CRTP console, the logging framework should be used for this. So we need to parse the lat/long/alt/fix data coming from the module and place into variables that can be logged. But there’s functionality that we would like that doesn’t fit within the logging/parameter framework, so a new gps port will be added. Using this port we are planning on making more data available (like information about satellites). But the main reason for this new port is to be able to send data to the GPS implementation in order to implement A-GPS to minimize the time to get a position fix. So by downloading the GPS almanac online and uploading via the radio to the Crazyflie the first time to fix should be shortened considerably.

If you would like to give it a try then have a look at the GPS hacks page om the Wiki for instructions. Note that on Ubuntu 13.10 (and probably other distros as well) the Marble build doesn’t include the Python bindings, so you will have to build Marble from source and enable them. If you would like to play around a bit with Marble here are some docs: Python examples and C++ API. If you don’t have a GPS module but still want to try it, then enable the DebugDriver. It will send fake lat/long/alt data to the UI. Oh, and if you figure out how to plot a path over the map, let us know ;-)

A quick note about dependencies for specific tabs in the Crazyflie python client. New tabs are added to the cfclient by creating a python file in the lib/cfclient/ui/tabs directory. So if you would like to add a tab for GPS you would just create a GpsTab.py file and this will automatically be picked up when the application starts up. Since we are now adding some dependencies that are just for specific tabs (like Marble for the GPS and PyQtGraph for the Plot) we have also added some decency checking. This means that if you don’t have Marble or PyQtGraph installed when starting the cfclient these tabs will still be listed in the menus, but will be disabled.

Finally, don’t forget about our holiday competition where you can win Crazyflies! There’s still one more week to go before it ends.

Response code is 404

3 comments on “Trying out GPS with the Crazyflie

  • I wrote some c++ code to parse the lon/lat just last night actually.

    It’s adapted from the arduino Adafruit code. The license is BSD, including my changes (my changes are to output longitude and latitude in deci-degrees * 1e7.

    boolean Adafruit_GPS::parseLonLat(char *&p) {
    // parse out latitude
    p = strchr(p, ‘,’)+1;
    if( p[0] == 0 || p[1] == 0)
    return false;
    int32_t latitude_degrees = (p[0]-‘0’)*10 + (p[1]-‘0’);
    p+=2;
    float latitude_minutes = atof(p);

    p = strchr(p, ‘,’)+1;
    if (p[0] == ‘N’) lat = ‘N’;
    else if (p[0] == ‘S’) lat = ‘S’;
    else if (p[0] == ‘,’) lat = 0;
    else return false;
    latitude = latitude_degrees*1e7 + ((int32_t)(latitude_minutes * 1e7) / 60);
    if(lat == ‘S’)
    latitude = -latitude;

    // parse out longitude
    p = strchr(p, ‘,’)+1;
    if( p[0] == 0 || p[1] == 0 || p[2] == 0)
    return false;
    int32_t longitude_degrees = (p[0]-‘0’)*100 + (p[1]-‘0’)*10 + (p[2]-‘0’);
    p+=3;
    float longitude_minutes = atof(p);

    p = strchr(p, ‘,’)+1;
    if (p[0] == ‘W’) lon = ‘W’;
    else if (p[0] == ‘E’) lon = ‘E’;
    else if (p[0] == ‘,’) lon = 0;
    else return false;
    longitude = longitude_degrees*1e7 + ((int32_t)(longitude_minutes *1e7) / 60);
    if(lon ==’W’)
    longitude =-longitude;
    return true;
    }

    boolean Adafruit_GPS::parse(char *nmea) {
    // do checksum check

    // first look if we even have one
    if (nmea[strlen(nmea)-4] == ‘*’) {
    uint16_t sum = parseHex(nmea[strlen(nmea)-3]) * 16;
    sum += parseHex(nmea[strlen(nmea)-2]);

    // check checksum
    for (uint8_t i=1; i 0)
    fix = true;

    p = strchr(p, ‘,’)+1;
    satellites = atoi(p);

    p = strchr(p, ‘,’)+1;
    HDOP = atof(p);

    p = strchr(p, ‘,’)+1;
    float altitudef = atof(p);
    altitude = altitudef*1000;
    p = strchr(p, ‘,’)+1;
    p = strchr(p, ‘,’)+1;
    float geoidheightf = atof(p);
    geoidheight = geoidheightf*1000;
    return true;
    }
    if (strstr(nmea, “$GPRMC”)) {
    // found RMC
    char *p = nmea;

    // get time
    p = strchr(p, ‘,’)+1;
    float timef = atof(p);
    uint32_t time = timef;
    hour = time / 10000;
    minute = (time % 10000) / 100;
    seconds = (time % 100);

    milliseconds = fmod(timef, 1.0) * 1000;

    p = strchr(p, ‘,’)+1;
    // Serial.println(p);
    if (p[0] == ‘A’)
    fix = true;
    else if (p[0] == ‘V’)
    fix = false;
    else
    return false;

    if(!parseLonLat(p))
    return false;

    // speed
    p = strchr(p, ‘,’)+1;
    speed = atof(p);

    // angle
    p = strchr(p, ‘,’)+1;
    angle = atof(p);

    p = strchr(p, ‘,’)+1;
    uint32_t fulldate = atof(p);
    day = fulldate / 10000;
    month = (fulldate % 10000) / 100;
    year = (fulldate % 100);

    // we dont parse the remaining, yet!
    return true;
    }

    if (strstr(nmea, “$GPGSA”)) {
    // found GSA
    char *p = nmea;
    p = strchr(p, ‘,’)+1;
    mode = p[2]; /* 1 = Fix not available. 2 = 2D. 3 = 3D */
    if (mode 3)
    return false;
    // we dont parse the remaining, yet!
    return true;
    }
    return false;
    }

    char Adafruit_GPS::read(void) {
    char c = 0;

    if (paused) return c;

    #ifdef __AVR__
    if(gpsSwSerial) {
    if(!gpsSwSerial->available()) return c;
    c = gpsSwSerial->read();
    }
    else
    {
    if(!gpsHwSerial->available()) return c;
    c = gpsHwSerial->read();
    }
    #else
    // if(!gpsHwSerial->available()) return c;
    // c = gpsHwSerial->read();
    if(!Serial1.available()) return c;
    c = Serial1.read();
    #endif

    //Serial.print(c);

    if (c == ‘$’) {
    currentline[lineidx] = 0;
    lineidx = 0;
    }
    if (c == ‘\n’) {
    currentline[lineidx] = 0;

    if (currentline == line1) {
    currentline = line2;
    lastline = line1;
    } else {
    currentline = line1;
    lastline = line2;
    }

    //Serial.println(“—-“);
    //Serial.println((char *)lastline);
    //Serial.println(“—-“);
    lineidx = 0;
    recvdflag = true;
    }

    currentline[lineidx++] = c;
    if (lineidx >= MAXLINELENGTH)
    lineidx = MAXLINELENGTH-1;

    return c;
    }

    #ifdef __AVR__
    // Constructor when using SoftwareSerial or NewSoftSerial
    #if ARDUINO >= 100
    Adafruit_GPS::Adafruit_GPS(SoftwareSerial *ser)
    #else
    Adafruit_GPS::Adafruit_GPS(NewSoftSerial *ser)
    #endif
    {
    common_init(); // Set everything to common state, then…
    gpsSwSerial = ser; // …override gpsSwSerial with value passed.
    }
    #endif

    Adafruit_GPS::Adafruit_GPS(HardwareSerial *ser) {
    common_init(); // Set everything to common state, then…
    gpsHwSerial = ser; // …override gpsHwSerial with value passed.
    }

    // Initialization code used by all constructor types
    void Adafruit_GPS::common_init(void) {
    recvdflag = false;
    paused = false;
    lineidx = 0;
    currentline = line1;
    lastline = line2;

    hour = minute = seconds = year = month = day =
    fixquality = satellites = 0; // uint8_t
    lat = lon = mag = 0; // char
    fix = false; // boolean
    milliseconds = 0; // uint16_t
    latitude = longitude = geoidheight = altitude =
    speed = angle = magvariation = HDOP = 0.0; // float
    }

    void Adafruit_GPS::begin(uint16_t baud)
    {
    #ifdef __AVR__
    if(gpsSwSerial)
    gpsSwSerial->begin(baud);
    else
    gpsHwSerial->begin(baud);
    #else
    // gpsHwSerial->begin(baud);
    Serial1.begin(baud);
    #endif

    delay(10);
    }

    void Adafruit_GPS::sendCommand(char *str) {
    #ifdef __AVR__
    if(gpsSwSerial)
    gpsSwSerial->println(str);
    else
    gpsHwSerial->println(str);
    #else
    // gpsHwSerial->println(str);
    Serial1.println(str);
    #endif

    }

    boolean Adafruit_GPS::newNMEAreceived(void) {
    return recvdflag;
    }

    void Adafruit_GPS::pause(boolean p) {
    paused = p;
    }

    char *Adafruit_GPS::lastNMEA(void) {
    recvdflag = false;
    return (char *)lastline;
    }

    // read a Hex value and return the decimal equivalent
    uint8_t Adafruit_GPS::parseHex(char c) {
    if (c < '0')
    return 0;
    if (c <= '9')
    return c – '0';
    if (c < 'A')
    return 0;
    if (c <= 'F')
    return (c – 'A')+10;
    }

    boolean Adafruit_GPS::waitForSentence(char *wait4me, uint8_t max) {
    char str[20];

    uint8_t i=0;
    while (i < max) {
    if (newNMEAreceived()) {
    char *nmea = lastNMEA();
    strncpy(str, nmea, 20);
    str[19] = 0;
    i++;

    if (strstr(str, wait4me))
    return true;
    }
    }

    return false;
    }

    boolean Adafruit_GPS::LOCUS_StartLogger(void) {
    sendCommand(PMTK_LOCUS_STARTLOG);
    recvdflag = false;
    return waitForSentence(PMTK_LOCUS_LOGSTARTED);
    }

    boolean Adafruit_GPS::LOCUS_ReadStatus(void) {
    sendCommand(PMTK_LOCUS_QUERY_STATUS);

    if (! waitForSentence("$PMTKLOG"))
    return false;

    char *response = lastNMEA();
    uint16_t parsed[10];
    uint8_t i;

    for (i=0; i<10; i++) parsed[i] = -1;

    response = strchr(response, ',');
    for (i=0; i http://www.adafruit.com/products/746
    Pick one up today at the Adafruit electronics shop
    and help support open source hardware & software! -ada

    Adafruit invests time and resources providing this open source code,
    please support Adafruit and open-source hardware by purchasing
    products from Adafruit!

    Written by Limor Fried/Ladyada for Adafruit Industries.
    BSD license, check license.txt for more information
    All text above must be included in any redistribution
    ****************************************/

    #ifndef _ADAFRUIT_GPS_H
    #define _ADAFRUIT_GPS_H

    #ifdef __AVR__
    #if ARDUINO >= 100
    #include
    #else
    #include
    #endif
    #endif

    // different commands to set the update rate from once a second (1 Hz) to 10 times a second (10Hz)
    #define PMTK_SET_NMEA_UPDATE_1HZ “$PMTK220,1000*1F”
    #define PMTK_SET_NMEA_UPDATE_5HZ “$PMTK220,200*2C”
    #define PMTK_SET_NMEA_UPDATE_10HZ “$PMTK220,100*2F”

    #define PMTK_SET_BAUD_57600 “$PMTK251,57600*2C”
    #define PMTK_SET_BAUD_9600 “$PMTK251,9600*17”

    // turn on only the second sentence (GPRMC)
    #define PMTK_SET_NMEA_OUTPUT_RMCONLY “$PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29”
    // turn on GPRMC and GGA and GSA, with GSA only every 5th transmission
    // Use http://www.hhhh.org/wiml/proj/nmeaxor.html to find the 2 digit checksum
    #define PMTK_SET_NMEA_OUTPUT_RMCGGA “$PMTK314,0,1,0,1,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2D”
    #define PMTK_SET_NMEA_OUTPUT_GGAONLY “$PMTK314,0,0,0,1,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2C”
    // turn on ALL THE DATA
    #define PMTK_SET_NMEA_OUTPUT_ALLDATA “$PMTK314,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28”
    // turn off output
    #define PMTK_SET_NMEA_OUTPUT_OFF “$PMTK314,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28”

    // to generate your own sentences, check out the MTK command datasheet and use a checksum calculator
    // such as the awesome http://www.hhhh.org/wiml/proj/nmeaxor.html

    #define PMTK_LOCUS_STARTLOG “$PMTK185,0*22”
    #define PMTK_LOCUS_LOGSTARTED “$PMTK001,185,3*3C”
    #define PMTK_LOCUS_QUERY_STATUS “$PMTK183*38”
    #define PMTK_LOCUS_ERASE_FLASH “$PMTK184,1*22”
    #define LOCUS_OVERLAP 0
    #define LOCUS_FULLSTOP 1

    // standby command & boot successful message
    #define PMTK_STANDBY “$PMTK161,0*28”
    #define PMTK_STANDBY_SUCCESS “$PMTK001,161,3*36” // Not needed currently
    #define PMTK_AWAKE “$PMTK010,002*2D”

    // ask for the release and version
    #define PMTK_Q_RELEASE “$PMTK605*31”

    // request for updates on antenna status
    #define PGCMD_ANTENNA “$PGCMD,33,1*6C”
    #define PGCMD_NOANTENNA “$PGCMD,33,0*6C”

    // how long to wait when we’re looking for a response
    #define MAXWAITSENTENCE 5

    #if ARDUINO >= 100
    #include “Arduino.h”
    #if defined (__AVR__) && !defined(__AVR_ATmega32U4__)
    #include “SoftwareSerial.h”
    #endif
    #else
    #include “WProgram.h”
    #include “NewSoftSerial.h”
    #endif

    class Adafruit_GPS {
    public:
    void begin(uint16_t baud);
    Adafruit_GPS(HardwareSerial *ser); // Constructor when using HardwareSerial

    char *lastNMEA(void);
    boolean newNMEAreceived();
    void common_init(void);
    void sendCommand(char *);
    void pause(boolean b);

    boolean parseNMEA(char *response);
    uint8_t parseHex(char c);

    char read(void);
    boolean parseLonLat(char *&);
    boolean parse(char *);
    void interruptReads(boolean r);

    boolean wakeup(void);
    boolean standby(void);

    uint8_t hour, minute, seconds, year, month, day;
    uint16_t milliseconds;
    /* latitude and longitude are in centi degrees *1e7 */
    int32_t latitude, longitude;
    /* Altitude in mm */
    int32_t geoidheight, altitude;
    float speed, angle, magvariation, HDOP;
    char lat, lon, mag;
    boolean fix;
    uint8_t fixquality, satellites;
    char mode; /* 1 = Fix not available. 2 = 2D. 3 = 3D */

    boolean waitForSentence(char *wait, uint8_t max = MAXWAITSENTENCE);
    boolean LOCUS_StartLogger(void);
    boolean LOCUS_ReadStatus(void);

    uint16_t LOCUS_serial, LOCUS_records;
    uint8_t LOCUS_type, LOCUS_mode, LOCUS_config, LOCUS_interval, LOCUS_distance, LOCUS_speed, LOCUS_status, LOCUS_percent;
    private:
    boolean paused;

    uint8_t parseResponse(char *response);
    #ifdef __AVR__
    #if ARDUINO >= 100
    SoftwareSerial *gpsSwSerial;
    #else
    NewSoftSerial *gpsSwSerial;
    #endif
    #endif
    HardwareSerial *gpsHwSerial;
    };

    #endif

  • Hi Marcus,

    I am currently developing an autonomous drone based off of the Crazyflie 2.0. I have ordered a GPS, and I need to begin interfacing it with the CF UART module. Can you share your code, or explain to me how to setup the UART interface in the crazyflie microcontroller firmware?

    -Tevon

Leave a Reply

Your email address will not be published. Required fields are marked *