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.
[pe2-gallery album=”http://picasaweb.google.com/data/feed/base/user/115721472821530986219/albumid/5964703597351683761?alt=rss&hl=en_US&kind=photo” ]
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
I would like to suggest WebGL Earth:
http://www.webglearth.com/
https://github.com/webglearth/webglearth2
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