Arduino GPS waypoints nav for UAVS FINAL!!!

Hello people (happy drunkdays with the family), im glad to announce my final version (no beta) of my Arduino GPS navigation, with a lot of improvements and now is commented in English :o.
In this pic, you can se the accuracy, have an error of 2 meters (simulation):
http://jordii07.googlepages.com/NEWgps.jpg
My real destination was "Casa", and i passed over it, almost perfect. Enough for my arduplane.

The second surprise is that i developed a ground station with labview, you can use any serial modem you want, pic:
http://jordii07.googlepages.com/ground.jpg

know issues:
-Not work if your try to navigate from latitude north to latitude south or vise versa, the same with longitude, but, if you live in the USA or Mexico, you going to have no problems, (poor Ecuador's and Colombians). You can fix it easily, but you need some extra code (useless for me).
-The max range of the distance is 65535 meters (6.5 KM) , unless you change the unsigned integers to a long integers. (i dont have the balls to fly that distance with my RC), the accuracy is very nice.
-Error of 2 meters + GPS device error, between 5 to 10 meter of accuracy (better you use a ublox).

• Some bug in the ground station software, is you dont stop the ports, when you try to close the program it will crash, no big deal, but better you know. Letter ill fix it >:(.
  -The altitude from GPS no work any more,i removed it, because have an error of 20 meters (Crash for sure). But right now i received a pressure sensor (free sample) from freescale (MPXA6115A), with a precision of 8 cm (almost 3 inches). Using an ADC of 16 bit resolution with i2C interface (MAX1169).

Improvements:
-Code less complex
-More accuracy resolution (heading degrees).
-Arduino Forward the NMEA sentences, and came with a new one, YEAH the $JORDI (in honor of my grandpa :P) sentences, look like this:

$GPRMC,044756,A,3414.4200,N,11826.9260,W,019.0,105.0,301207,000.0,W7C
$JORDI,240,187,19,34240332,-118448768,105,888,-1738,3,150 <------No check sum hehe ;D
$GPVTG,105.0,T,105.0,M,019.0,N,035.2,K42
$GPZDA,044756,30,12,2007,00*65
$GPGGA,044756,3414.4200,N,11826.9260,W,2,08,2.0,0,M,50.0,M,,43
$GPGSA,A,3,01,02,03,,05,,07,,09,,11,12,2.0,2.0,2.038
were:
$JORDI,Course to Destination, Distance, Speed, Latitude,Longitud, Real Course,Dif Lat, Dif Long, Quadrant, Correction

You can change it if you one, but is used by the ground station, to read the data. Then the ground station forward again the NMEA sentence to another port, to use it for example in Google Earth, etc and see your position in real time. Google Earth ignores the $JORDI sentence.

You can download the complete sketch and the labview source code from here:
http://jordii07.googlepages.com/GPSarduinov101.rar

And please, pleaseee!!!!!! :'(, try to read and understand this documents:
http://jordii07.googlepages.com/Waypoint-Math.pdf
The navigation part is based in this document, exactly... :
and this;
Arduino Playground - GPS esperabas

Good luck, i know... i use to make lot of mistakes, but what you expect from a 21 years old, trying to write in another language???..

Anyway, i just do it to pay back for all the arduino community made for me, thanks... :-*

Made by a Mexican Tijuanero...

Wow, great achievement. Your projects have been among the best I have seen here on the forums!

As I am a r/c modeller too, could you please post a picture of your Arduplane?

Sorry im a super poor student.. right now i only have a Sabre RC helicopter (100 dlls), very unstable. And old airplane far away from me (in my Old house in Mexico).
Right now im focusing in this:
http://www.hobbiconexstar.com/nexstar-select/index.html
Is perfect and includes everything, even a simulator to train yourself.

I have 100 dlls in my pocket right now, i need 300 more. Im selling my Nintendo Wii, with 3 games (Including Zelda, trauma center, Call Of duty, and nintendo game cube Resident evil 4), works perfectly, i used it less than 40 hours. FOr only $349.99 (with original boxes), i Live between RIverside and Los Angeles, California... Support to continue my UAVS project.... THANKS!!!!
More info. jordii((@))live.com

by the way i upload a video of my GPS working.... (i used myspace because youtube dont let me upload more than 10 mins):
http://myspacetv.com/index.cfm?fuseaction=vids.individual&videoid=25046068
Happy New Year!!!!!

sorry, its hard for me to watch the vid cuz i really want to understand what your saying, but i dont understand your language. Can you make an english version, becuase this is really cool, but i wanna comprende you! ( lol )

also, are you making a helicopter that uses gps coordinates to move, because that would be crzy awsome, and one more thing:

can you link me to where you got the gps receiver? because i want a cheap receiver to do this kind of stuff with.
great job on the project, it looks awsome!

Right now i NOT going to use any helicopter, to use an helicopter i need a complex hardware, and MONEY. Right now ill use and airplane because is shipper (less complex hardware) :D. But someday, i will try again with an helicopter.
About the video, i able to read and understand very well english, im able to write (with mistakes) and take me some time. But TALK haha :P!!!!. :-[. Maybe i able to have simple conversations, but explain something like this :-/. Let see what i can do, ill ask to my best friend, he speaks pretty well, but he lives in San Ysidro, 3 hours from here. Ill going to visit him in one week, because he is going to help me with the kalman filters for the IMU of Arduino (and eat tacos in Tijuana lol). 8-). Im doing this project because i have all the time of the world, (im waiting for my green card, (i cant work, study, etc.) until January 23), i been here in usa for about 7 months (pretty boring). But that situation give me all the time to investigate (mixed with 1 year of digital electronic engineering in mx University ;D).

If you have any question ask me please.
GPS; I recommend you the UBLOx, with 4MHZ refresh rate and i2c (twi) technology. And is the only one with 50 CHANNELS!!!!!!. For only 99 dlls.
http://www.abacuscity.ch/abashop?s=142&p=productdetail&sku=78

For shipper version of gps check out sparkfun:

But remember you got what you pay. For an airplane always buy the expensive sensors. Good sensors= Good Data= And Arduino with Good data = success.

i under estimate Atmega 168 (arduino heart), but if you know how to programming it well is pretty powerful.

whoa! 100 bucks, thats alot of money, buti have to agree, 50 channels will kepp anybody busy, but what are channels used for with gps recievers?
do the channels tell height and altitude and all that jazz? or is it other data?

oh yea, and like my question before, are you going to use a gps to make a plane fly to certian places using gps, becuase that would be awsome!

lol i just changed helicopter with airplane

also be carefull, i'd say use a cheaper gps for the test flights, and only use ur 100 dollar gps for full blown flights, becuase planes tend to crash, not becuase of bad circuitry, but becuase planes are planes, the gps could flip out and transmit the VERY wrong altitude, sending the plane into a crazy dive, and then again, tress do the same damage, so dont go crashing planes with 100 dollar gps modules installed lol

Unless the airplane explode (fire), i think the GPS could survive (is just an rc trainer with speed brakes), may put all the sensors in a black box, with an external antenna to protect them. :
I dont know.. i have a lot of time to develop this. By the way i wish the ublox..... off course, but for now ill buy this one:

(they asked me: What i recommend??, i answer with the truth ).
Letter ill buy the ublox.

My firsts tests going to be with all the hardware mounted in the plane but i not going to use Autopilot, just "see" the attitude of the system/sensors, i going to read and log the data with the ground station made with labview. (short fly).
Until i feel comfortable with the results ill test the autopilot. Off course ill post my results.

Im finishing my IMU with kalman filters, check this out:

#include <math.h>

unsigned long lastread = 0; //time reading to control dt.
int mydt = 20; //in ms.
int dt = 0.02;

int RATEpin = 2; //IMU pins.
int Y_ACCELpin = 1;
int X_ACCELpin = 0;

int RATEbits; //Bit Conversion
int Y_ACCELbits;
int X_ACCELbits;

float AX; //Value in g's
float AY;

float angle_m; //Engineering Units in degrees and degrees/sec
float q_m;
float CALC_ANGLE;

//Kalman Filter ShizNITE!

static float P[2][2] = {
{ 1, 0 },
{ 0, 1 },
};

float angle;
float q_bias;
float rate;

static const float R_angle = .0034906555555555555556;
static const float Q_angle = 0.001;
static const float Q_gyro = 0.003;

void setup() {
pinMode(RATEpin, INPUT);
pinMode(Y_ACCELpin, INPUT);
pinMode(X_ACCELpin, INPUT);
Serial.begin(115200);
}

void loop() {

if((millis()-lastread) >= mydt) { // sample every dt ms -> 1000/dt hz.
lastread = millis();

//Gyro Measurement
RATEbits = analogRead(RATEpin);
q_m = ((RATEbits - 512)*.0056814055266203703704);

//Accelerometer Measurement
Y_ACCELbits = analogRead(Y_ACCELpin);
X_ACCELbits = analogRead(X_ACCELpin);
AX = (512 - X_ACCELbits)/2.56;
AY = (512 - Y_ACCELbits)/2.56;
angle_m = -(atan2(-AY, AX));

//Kalman Code Goes Here!
const float q = q_m - q_bias;

const float Pdot[2 * 2] = {Q_angle - P[0][1] - P[1][0], - P[1][1], - P[1][1], Q_gyro };

rate = q;

angle += q * dt;

P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

const float angle_err = angle_m - angle;

const float C_0 = 1;

const float PCt_0 = C_0 * P[0][0];
const float PCt_1 = C_0 * P[1][0];

const float E =R_angle+ C_0 * PCt_0;

const float K_0 = PCt_0 / E;
const float K_1 = PCt_1 / E;

const float t_0 = PCt_0;
const float t_1 = C_0 * P[0][1];

P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;
q_bias += K_1 * angle_err;


Serial.print(int ((angle*180)/PI));
Serial.print(",");
Serial.print(int ((rate*180)/PI));
Serial.print(",");
Serial.print(int (((q_bias*100)*180)/PI));
Serial.print(",");
Serial.print(int(angle_m));
Serial.print(",");
Serial.print(int(q_m));
Serial.print(",");
Serial.println("");

}
}

Unless the airplane explode (fire), i think the GPS could survive (is just an rc trainer with speed brakes), may put all the sensors in a black box, with an external antenna to protect them. :

Have you considered electric flight? An explosion won't happen as soon with a battery as with fuel...

yea, i think he has an electric plane.

but now you also have to weigh the plane right now ( before any hacks ) and make sure that later, you dont exceed that weight, or your plane will have to stain the motor just to keep it up, and the black box idea is a good idea, maybe add a thin layer of insilater ( bubble wrap, or foam or something ) and line the box with it, or line the electroncs with it, but adding the box would help you recover your electronics in one piece lol
And that gps seems fairly priced, ill save up and buy it, but im also woundering:
does it tell me altitude?
speed?
is it easy to interface with the arduino?
do you know any of these answers?
also, what does 20 channels give me?

and
now the code that you posted, what does it do?

Unless the airplane explode (fire), i think the GPS could survive (is just an rc trainer with speed brakes), may put all the sensors in a black box, with an external antenna to protect them. :

Have you considered electric flight? An explosion won't happen as soon with a battery as with fuel...

No... i don't going to use electric for this project, the average time fly of an electric airplane is about 15 to 20 minutes, i need more. So ill you gas airplaine...

yea, i think he has an electric plane.

but now you also have to weigh the plane right now ( before any hacks ) and make sure that later, you dont exceed that weight, or your plane will have to stain the motor just to keep it up, and the black box idea is a good idea, maybe add a thin layer of insilater ( bubble wrap, or foam or something ) and line the box with it, or line the electroncs with it, but adding the box would help you recover your electronics in one piece lol
And that gps seems fairly priced, ill save up and buy it, but im also woundering:
does it tell me altitude?
speed?
is it easy to interface with the arduino?
do you know any of these answers?
also, what does 20 channels give me?

and
now the code that you posted, what does it do?

Yes it tell your altitude, but with accuracy's of +- 15 meters. , so better use a pressure sensor.
The speed is very accurate (error of .01 knots)
Yes is easy interface it with arduino, with my code for GPS navigation (UP this post) you just, connect it to pin 1 (TTL serial) and it will work.

About GPS is very relative, but i gone give an idea:

if you have 20 channels maybe in a real world you gonna have 5 to 10 meters of error with your real position
30 channels i hear about 3 to 5 meters
and 50 channels like ublox some people say that is less than 2 meters. (but letter this GPS is going to be more accurate, because is build with the new GPS technology called Galileo from Europe: Galileo (satellite navigation) - Wikipedia)

Instructions:
1-Upload my code to arduino
2-Connect your GPS just like this:

And you are done... Don't forget to program your destination coordinates.

what do i have to call to get lat / long, and altitude? and other stuff?

You have to extract lat/lon information from the serial stream of data that is coming from the gps

oh, ok it'll be a serial input....

Yes... but, you don't need to worry about the lat and long. The code already process the NMEA sentences for you

i cant promise ill have the gps soon, even though i want it soon... im on a tight budget, and it's either that gps, or a lot of electronics, and i'm intrested in learning simpler things like sensors, and lcd screens, so i can post the gps info on a screen!

Wuju:

http://www.hackszine.com/blog/archive/2008/01/arducopter.html?CMP=OTC-7G2N43923558
Making promotion to arduino ;D

i cant promise ill have the gps soon, even though i want it soon... im on a tight budget, and it's either that gps, or a lot of electronics, and i'm intrested in learning simpler things like sensors, and lcd screens, so i can post the gps info on a screen!

Sparkfun does sell the gps for US$ 99.95, but you can get it here for a lot less (US$ 55.00): http://www.ohararp.com/products.html

which gps is that?

cuz the one with 20 chans at spark are only 60...

Well, in my country 60 is still more than 55 ;D (sorry, couldn't resist)

I am talking about the ETek EB-85A with 5Hz engine, sold for 99.95 at Sparkfun, but only 55 http://www.ohararp.com/products.html