Joseph Roberts / Mbed 2 deprecated Quadcopter_mk2

New version of quadcopter software written to OO principles

Dependencies:   mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS

Sensors/Gps/Gps.cpp@4:9ffbf9101992, 2015-05-08 (annotated)

Committer:
joe4465
Date:
Fri May 08 09:07:38 2015 +0000
Revision:
4:9ffbf9101992
Parent:
3:4823d6750629 
End of FYP

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 2:969dfa4f2436 1 #include "Gps.h"
joe4465 2:969dfa4f2436 2
joe4465 2:969dfa4f2436 3 Gps::Gps(PinName gpsPinTx, PinName gpsPinRx)
joe4465 2:969dfa4f2436 4 {
joe4465 2:969dfa4f2436 5 _values = Value();
joe4465 2:969dfa4f2436 6 _gps = new MODSERIAL(gpsPinTx, gpsPinRx);
joe4465 2:969dfa4f2436 7 _gps->baud(115200);
joe4465 2:969dfa4f2436 8
joe4465 2:969dfa4f2436 9 DEBUG("GPS initialised\r\n");
joe4465 2:969dfa4f2436 10 }
joe4465 2:969dfa4f2436 11
joe4465 2:969dfa4f2436 12 Gps::~Gps(){}
joe4465 2:969dfa4f2436 13
joe4465 2:969dfa4f2436 14 Gps::Value Gps::getValues()
joe4465 2:969dfa4f2436 15 {
joe4465 2:969dfa4f2436 16 while(_gps->readable() > 0)
joe4465 2:969dfa4f2436 17 {
joe4465 2:969dfa4f2436 18 int c = _gps->getc();
joe4465 2:969dfa4f2436 19 if(_tinyGPS.encode(c))
joe4465 2:969dfa4f2436 20 {
joe4465 2:969dfa4f2436 21 unsigned long fix_age;
joe4465 2:969dfa4f2436 22 _tinyGPS.f_get_position(&_values.latitude, &_values.longitude, &fix_age);
joe4465 2:969dfa4f2436 23
joe4465 2:969dfa4f2436 24 _values.altitude = _tinyGPS.f_altitude();
joe4465 2:969dfa4f2436 25
joe4465 2:969dfa4f2436 26 if ((fix_age == TinyGPS::GPS_INVALID_AGE) || (fix_age > 5000)) _values.fix = false;
joe4465 2:969dfa4f2436 27 else _values.fix = true;
joe4465 2:969dfa4f2436 28 }
joe4465 2:969dfa4f2436 29 }
joe4465 2:969dfa4f2436 30
joe4465 2:969dfa4f2436 31 return _values;
joe4465 3:4823d6750629 32 }
joe4465 3:4823d6750629 33
joe4465 3:4823d6750629 34 Gps::Difference Gps::getDifference(Value latLong1, Value latLong2)
joe4465 3:4823d6750629 35 {
joe4465 3:4823d6750629 36 int R = 637100000; //cm
joe4465 3:4823d6750629 37 float dLat = deg2rad(latLong2.latitude - latLong1.latitude);
joe4465 3:4823d6750629 38 float dLon = deg2rad(latLong2.longitude - latLong1.longitude);
joe4465 3:4823d6750629 39 latLong1.latitude = deg2rad(latLong1.latitude);
joe4465 3:4823d6750629 40 latLong2.latitude = deg2rad(latLong2.latitude);
joe4465 3:4823d6750629 41
joe4465 3:4823d6750629 42 float a = sin(dLat/2) * sin(dLat/2) + sin(dLon/2) * sin(dLon/2) * cos(latLong1.latitude) * cos(latLong2.latitude);
joe4465 3:4823d6750629 43 float c = 2 * atan2(sqrt(a), sqrt(1-a));
joe4465 3:4823d6750629 44 float d = R * c;
joe4465 3:4823d6750629 45
joe4465 3:4823d6750629 46 float y = sin(dLon) * cos(latLong2.latitude);
joe4465 3:4823d6750629 47 float x = cos(latLong1.latitude) * sin(latLong2.latitude) - sin(latLong1.latitude) * cos(latLong2.latitude) * cos(dLat);
joe4465 3:4823d6750629 48 float brng = rad2deg(atan2(y, x));
joe4465 3:4823d6750629 49
joe4465 3:4823d6750629 50 float oppAng = 90 - brng;
joe4465 3:4823d6750629 51 float xDiff = d * sin(deg2rad(brng));
joe4465 3:4823d6750629 52 float yDiff = d * sin(deg2rad(oppAng));
joe4465 3:4823d6750629 53
joe4465 3:4823d6750629 54 //printf("d %f, opp %f, diff %f \r\n", d, oppAng, yDiff);
joe4465 3:4823d6750629 55 //printf("lat1 %f long1 %f lat2 %f long2 %f ", latLong1.latitude, latLong1.longitude, latLong2.latitude, latLong2.longitude);
joe4465 3:4823d6750629 56 //printf(" Bearing %f", brng);
joe4465 3:4823d6750629 57 //printf(" Distance %f", d);
joe4465 3:4823d6750629 58 //printf(" X %f, Y %f\r\n", xDiff, yDiff);
joe4465 3:4823d6750629 59
joe4465 3:4823d6750629 60 Difference difference = Difference();
joe4465 3:4823d6750629 61 difference.x = xDiff;
joe4465 3:4823d6750629 62 difference.y = yDiff;
joe4465 3:4823d6750629 63 difference.bearing = brng;
joe4465 3:4823d6750629 64 difference.distance = d;
joe4465 3:4823d6750629 65
joe4465 3:4823d6750629 66 return difference;
joe4465 3:4823d6750629 67 }
joe4465 3:4823d6750629 68
joe4465 3:4823d6750629 69 double Gps::deg2rad(double deg)
joe4465 3:4823d6750629 70 {
joe4465 3:4823d6750629 71 return (deg * PI / 180.0);
joe4465 3:4823d6750629 72 }
joe4465 3:4823d6750629 73
joe4465 3:4823d6750629 74 double Gps::rad2deg(double rad)
joe4465 3:4823d6750629 75 {
joe4465 3:4823d6750629 76 return (rad * 180.0 / PI);
joe4465 2:969dfa4f2436 77 }