FTE-denshi / Mbed 2 deprecated FTE-06

東北大学学友会準加盟団体 From The Earth の高高度ロケットFTE-06（通称：海豚）にて使用したソフトウェアです．ご自由にお使いください．このプログラムによって生じた損害について当団体は一切責任を負いません．また，各モジュールのライブラリは当団体が作成したものではないので再配布は禁止します．

Dependencies:   mbed FATFileSystem

Fork of FTE-06 by Tetsushi Amano

GPS/GPS.cpp@57:d78497a079cf, 2017-08-25 (annotated)

Committer:
mizuki_akaike
Date:
Fri Aug 25 03:42:30 2017 +0000
Revision:
57:d78497a079cf
Parent:
52:3da3fa088452 
??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mizuki_akaike 46:a8617076f021 1 #include "GPS.h"
mizuki_akaike 46:a8617076f021 2
mizuki_akaike 46:a8617076f021 3 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
mizuki_akaike 46:a8617076f021 4 _gps.baud(9600);
mizuki_akaike 46:a8617076f021 5 nmea_longitude = 0.0;
mizuki_akaike 46:a8617076f021 6 nmea_latitude = 0.0;
mizuki_akaike 46:a8617076f021 7 utc_time = 0;
mizuki_akaike 46:a8617076f021 8 ns = ' ';
mizuki_akaike 46:a8617076f021 9 ew = ' ';
mizuki_akaike 46:a8617076f021 10 lock = 0;
mizuki_akaike 46:a8617076f021 11 satelites = 0;
mizuki_akaike 46:a8617076f021 12 hdop = 0.0;
mizuki_akaike 46:a8617076f021 13 msl_altitude = 0.0;
mizuki_akaike 46:a8617076f021 14 msl_units = ' ';
mizuki_akaike 46:a8617076f021 15
mizuki_akaike 46:a8617076f021 16 rmc_status = ' ';
mizuki_akaike 46:a8617076f021 17 speed_k = 0.0;
mizuki_akaike 46:a8617076f021 18 course_d = 0.0;
mizuki_akaike 46:a8617076f021 19 date = 0;
mizuki_akaike 46:a8617076f021 20
mizuki_akaike 46:a8617076f021 21 dec_longitude = 0.0;
mizuki_akaike 46:a8617076f021 22 dec_latitude = 0.0;
mizuki_akaike 46:a8617076f021 23
mizuki_akaike 46:a8617076f021 24 gll_status = ' ';
mizuki_akaike 46:a8617076f021 25
mizuki_akaike 46:a8617076f021 26 course_t = 0.0; // ground speed true
mizuki_akaike 46:a8617076f021 27 course_t_unit = ' ';
mizuki_akaike 46:a8617076f021 28 course_m = 0.0; // magnetic
mizuki_akaike 46:a8617076f021 29 course_m_unit = ' ';
mizuki_akaike 46:a8617076f021 30 speed_k_unit = ' ';
mizuki_akaike 46:a8617076f021 31 speed_km = 0.0; // speek km/hr
mizuki_akaike 46:a8617076f021 32 speed_km_unit = ' ';
mizuki_akaike 46:a8617076f021 33
mizuki_akaike 46:a8617076f021 34 altitude_ft = 0.0;
mizuki_akaike 46:a8617076f021 35 #ifdef OPEN_LOG
mizuki_akaike 46:a8617076f021 36 is_logging = false;
mizuki_akaike 46:a8617076f021 37 #endif
mizuki_akaike 46:a8617076f021 38 }
mizuki_akaike 46:a8617076f021 39
mizuki_akaike 46:a8617076f021 40 #ifdef OPEN_LOG
mizuki_akaike 46:a8617076f021 41 void GPS::start_log() {
mizuki_akaike 46:a8617076f021 42 is_logging = true;
mizuki_akaike 46:a8617076f021 43 }
mizuki_akaike 46:a8617076f021 44
mizuki_akaike 46:a8617076f021 45 void GPS::new_file(void) {
mizuki_akaike 46:a8617076f021 46 _openLog.newFile();
mizuki_akaike 46:a8617076f021 47 }
mizuki_akaike 46:a8617076f021 48
mizuki_akaike 46:a8617076f021 49 void GPS::stop_log(void) {
mizuki_akaike 46:a8617076f021 50 is_logging = false;
mizuki_akaike 46:a8617076f021 51 }
mizuki_akaike 46:a8617076f021 52 #endif
mizuki_akaike 46:a8617076f021 53
mizuki_akaike 46:a8617076f021 54 float GPS::nmea_to_dec(float deg_coord, char nsew) {
mizuki_akaike 46:a8617076f021 55 int degree = (int)(deg_coord/100);
mizuki_akaike 46:a8617076f021 56 float minutes = deg_coord - degree*100;
mizuki_akaike 46:a8617076f021 57 float dec_deg = minutes / 60;
mizuki_akaike 46:a8617076f021 58 float decimal = degree + dec_deg;
mizuki_akaike 46:a8617076f021 59 if (nsew == 'S' || nsew == 'W') { // return negative
mizuki_akaike 46:a8617076f021 60 decimal *= -1;
mizuki_akaike 46:a8617076f021 61 }
mizuki_akaike 46:a8617076f021 62 return decimal;
mizuki_akaike 46:a8617076f021 63 }
mizuki_akaike 46:a8617076f021 64
mizuki_akaike 46:a8617076f021 65 int GPS::sample() {
mizuki_akaike 46:a8617076f021 66 int line_parsed = 0;
mizuki_akaike 46:a8617076f021 67
mizuki_akaike 46:a8617076f021 68 if (_gps.readable()) {
mizuki_akaike 46:a8617076f021 69 getline();
mizuki_akaike 46:a8617076f021 70
mizuki_akaike 46:a8617076f021 71 #ifdef OPEN_LOG
mizuki_akaike 46:a8617076f021 72 if (is_logging && lock) {
mizuki_akaike 46:a8617076f021 73 format_for_log();
mizuki_akaike 46:a8617076f021 74 _openLog.write(bfr);
mizuki_akaike 46:a8617076f021 75 }
mizuki_akaike 46:a8617076f021 76 #endif
mizuki_akaike 51:af23f1fc4a63 77
mizuki_akaike 52:3da3fa088452 78 FILE *fp= fopen("/sd/mydir/gps_data.txt", "a");
mizuki_akaike 51:af23f1fc4a63 79 if(fp!=NULL){
mizuki_akaike 51:af23f1fc4a63 80 fprintf(fp,"%s\n",msg);
mizuki_akaike 51:af23f1fc4a63 81 fclose(fp);
mizuki_akaike 51:af23f1fc4a63 82 }
mizuki_akaike 46:a8617076f021 83 // Check if it is a GPGGA msg (matches both locked and non-locked msg)
mizuki_akaike 46:a8617076f021 84 if (sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f,%c", &utc_time, &nmea_latitude, &ns, &nmea_longitude, &ew, &lock, &satelites, &hdop, &msl_altitude, &msl_units) >= 1) {
mizuki_akaike 46:a8617076f021 85 line_parsed = GGA;
mizuki_akaike 46:a8617076f021 86 }
mizuki_akaike 46:a8617076f021 87 // Check if it is a GPRMC msg
mizuki_akaike 46:a8617076f021 88 else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%f,%d", &utc_time, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
mizuki_akaike 46:a8617076f021 89 line_parsed = RMC;
mizuki_akaike 46:a8617076f021 90 }
mizuki_akaike 46:a8617076f021 91 // GLL - Geographic Position-Lat/Lon
mizuki_akaike 46:a8617076f021 92 else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &nmea_latitude, &ns, &nmea_longitude, &ew, &utc_time, &gll_status) >= 1) {
mizuki_akaike 46:a8617076f021 93 line_parsed = GLL;
mizuki_akaike 46:a8617076f021 94 }
mizuki_akaike 46:a8617076f021 95 // VTG-Course Over Ground and Ground Speed
mizuki_akaike 46:a8617076f021 96 else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
mizuki_akaike 46:a8617076f021 97 line_parsed = VTG;
mizuki_akaike 46:a8617076f021 98 }
mizuki_akaike 46:a8617076f021 99
mizuki_akaike 46:a8617076f021 100 if(satelites == 0) {
mizuki_akaike 46:a8617076f021 101 lock = 0;
mizuki_akaike 46:a8617076f021 102 }
mizuki_akaike 46:a8617076f021 103 }
mizuki_akaike 46:a8617076f021 104 if (!lock) {
mizuki_akaike 46:a8617076f021 105 return NO_LOCK;
mizuki_akaike 46:a8617076f021 106 } else if (line_parsed) {
mizuki_akaike 46:a8617076f021 107 return line_parsed;
mizuki_akaike 46:a8617076f021 108 } else {
mizuki_akaike 46:a8617076f021 109 return NOT_PARSED;
mizuki_akaike 46:a8617076f021 110 }
mizuki_akaike 46:a8617076f021 111 }
mizuki_akaike 46:a8617076f021 112
mizuki_akaike 46:a8617076f021 113
mizuki_akaike 46:a8617076f021 114 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
mizuki_akaike 46:a8617076f021 115 float GPS::trunc(float v) {
mizuki_akaike 46:a8617076f021 116 if (v < 0.0) {
mizuki_akaike 46:a8617076f021 117 v*= -1.0;
mizuki_akaike 46:a8617076f021 118 v = floor(v);
mizuki_akaike 46:a8617076f021 119 v*=-1.0;
mizuki_akaike 46:a8617076f021 120 } else {
mizuki_akaike 46:a8617076f021 121 v = floor(v);
mizuki_akaike 46:a8617076f021 122 }
mizuki_akaike 46:a8617076f021 123 return v;
mizuki_akaike 46:a8617076f021 124 }
mizuki_akaike 46:a8617076f021 125
mizuki_akaike 46:a8617076f021 126 void GPS::getline() {
mizuki_akaike 46:a8617076f021 127 while (_gps.getc() != '$'); // wait for the start of a line
mizuki_akaike 46:a8617076f021 128 for (int i=0; i<1022; i++) {
mizuki_akaike 46:a8617076f021 129 msg[i] = _gps.getc();
mizuki_akaike 46:a8617076f021 130 if (msg[i] == '\r') {
mizuki_akaike 46:a8617076f021 131 msg[i] = 0;
mizuki_akaike 46:a8617076f021 132 return;
mizuki_akaike 50:0dbbd39a54bb 133 }else if(msg[i]=='$'){
mizuki_akaike 50:0dbbd39a54bb 134 msg[i]=0;
mizuki_akaike 50:0dbbd39a54bb 135 return;
mizuki_akaike 50:0dbbd39a54bb 136 }
mizuki_akaike 46:a8617076f021 137 }
mizuki_akaike 46:a8617076f021 138 error("Overflow in getline");
mizuki_akaike 46:a8617076f021 139 }
mizuki_akaike 46:a8617076f021 140
mizuki_akaike 46:a8617076f021 141 void GPS::format_for_log() {
mizuki_akaike 46:a8617076f021 142 bfr[0] = '$';
mizuki_akaike 46:a8617076f021 143 for (int i = 0; i < 1022; i++) {
mizuki_akaike 46:a8617076f021 144 bfr[i+1] = msg[i];
mizuki_akaike 46:a8617076f021 145 if (msg[i] == 0 || msg[i] =='$') {
mizuki_akaike 46:a8617076f021 146 bfr[i+1] = '\r';
mizuki_akaike 46:a8617076f021 147 bfr[i+2] = '\n';
mizuki_akaike 46:a8617076f021 148 bfr[i+3] = 0;
mizuki_akaike 46:a8617076f021 149 return;
mizuki_akaike 46:a8617076f021 150 }
mizuki_akaike 46:a8617076f021 151 }
mizuki_akaike 46:a8617076f021 152 error("Overflow in format");
mizuki_akaike 46:a8617076f021 153 }
mizuki_akaike 46:a8617076f021 154
mizuki_akaike 46:a8617076f021 155 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
mizuki_akaike 46:a8617076f021 156 float GPS::get_msl_altitude() {
mizuki_akaike 46:a8617076f021 157 if (!lock)
mizuki_akaike 46:a8617076f021 158 return 0.0;
mizuki_akaike 46:a8617076f021 159 else
mizuki_akaike 46:a8617076f021 160 return msl_altitude;
mizuki_akaike 46:a8617076f021 161 }
mizuki_akaike 46:a8617076f021 162
mizuki_akaike 46:a8617076f021 163 int GPS::get_satelites() {
mizuki_akaike 46:a8617076f021 164 if (!lock)
mizuki_akaike 46:a8617076f021 165 return 0;
mizuki_akaike 46:a8617076f021 166 else
mizuki_akaike 46:a8617076f021 167 return satelites;
mizuki_akaike 46:a8617076f021 168 }
mizuki_akaike 46:a8617076f021 169
mizuki_akaike 46:a8617076f021 170 float GPS::get_nmea_longitude() {
mizuki_akaike 46:a8617076f021 171 if (!lock)
mizuki_akaike 46:a8617076f021 172 return 0.0;
mizuki_akaike 46:a8617076f021 173 else
mizuki_akaike 46:a8617076f021 174 return nmea_longitude;
mizuki_akaike 46:a8617076f021 175 }
mizuki_akaike 46:a8617076f021 176
mizuki_akaike 46:a8617076f021 177 float GPS::get_dec_longitude() {
mizuki_akaike 46:a8617076f021 178 dec_longitude = nmea_to_dec(nmea_longitude, ew);
mizuki_akaike 46:a8617076f021 179 if (!lock)
mizuki_akaike 46:a8617076f021 180 return 0.0;
mizuki_akaike 46:a8617076f021 181 else
mizuki_akaike 46:a8617076f021 182 return dec_longitude;
mizuki_akaike 46:a8617076f021 183 }
mizuki_akaike 46:a8617076f021 184
mizuki_akaike 46:a8617076f021 185 float GPS::get_nmea_latitude() {
mizuki_akaike 46:a8617076f021 186 if (!lock)
mizuki_akaike 46:a8617076f021 187 return 0.0;
mizuki_akaike 46:a8617076f021 188 else
mizuki_akaike 46:a8617076f021 189 return nmea_latitude;
mizuki_akaike 46:a8617076f021 190 }
mizuki_akaike 46:a8617076f021 191
mizuki_akaike 46:a8617076f021 192 float GPS::get_dec_latitude() {
mizuki_akaike 46:a8617076f021 193 dec_latitude = nmea_to_dec(nmea_latitude, ns);
mizuki_akaike 46:a8617076f021 194 if (!lock)
mizuki_akaike 46:a8617076f021 195 return 0.0;
mizuki_akaike 46:a8617076f021 196 else
mizuki_akaike 46:a8617076f021 197 return dec_latitude;
mizuki_akaike 46:a8617076f021 198 }
mizuki_akaike 46:a8617076f021 199
mizuki_akaike 46:a8617076f021 200 float GPS::get_course_t() {
mizuki_akaike 46:a8617076f021 201 if (!lock)
mizuki_akaike 46:a8617076f021 202 return 0.0;
mizuki_akaike 46:a8617076f021 203 else
mizuki_akaike 46:a8617076f021 204 return course_t;
mizuki_akaike 46:a8617076f021 205 }
mizuki_akaike 46:a8617076f021 206
mizuki_akaike 46:a8617076f021 207 float GPS::get_course_m() {
mizuki_akaike 46:a8617076f021 208 if (!lock)
mizuki_akaike 46:a8617076f021 209 return 0.0;
mizuki_akaike 46:a8617076f021 210 else
mizuki_akaike 46:a8617076f021 211 return course_m;
mizuki_akaike 46:a8617076f021 212 }
mizuki_akaike 46:a8617076f021 213
mizuki_akaike 46:a8617076f021 214 float GPS::get_speed_k() {
mizuki_akaike 46:a8617076f021 215 if (!lock)
mizuki_akaike 46:a8617076f021 216 return 0.0;
mizuki_akaike 46:a8617076f021 217 else
mizuki_akaike 46:a8617076f021 218 return speed_k;
mizuki_akaike 46:a8617076f021 219 }
mizuki_akaike 46:a8617076f021 220
mizuki_akaike 46:a8617076f021 221 float GPS::get_speed_km() {
mizuki_akaike 46:a8617076f021 222 if (!lock)
mizuki_akaike 46:a8617076f021 223 return 0.0;
mizuki_akaike 46:a8617076f021 224 else
mizuki_akaike 46:a8617076f021 225 return speed_km;
mizuki_akaike 46:a8617076f021 226 }
mizuki_akaike 46:a8617076f021 227
mizuki_akaike 46:a8617076f021 228 float GPS::get_altitude_ft() {
mizuki_akaike 46:a8617076f021 229 if (!lock)
mizuki_akaike 46:a8617076f021 230 return 0.0;
mizuki_akaike 46:a8617076f021 231 else
mizuki_akaike 46:a8617076f021 232 return 3.280839895*msl_altitude;
mizuki_akaike 46:a8617076f021 233 }
mizuki_akaike 46:a8617076f021 234
mizuki_akaike 46:a8617076f021 235 // NAVIGATION FUNCTIONS ////////////////////////////////////////////////////////////
mizuki_akaike 46:a8617076f021 236 float GPS::calc_course_to(float pointLat, float pontLong) {
mizuki_akaike 46:a8617076f021 237 const double d2r = PI / 180.0;
mizuki_akaike 46:a8617076f021 238 const double r2d = 180.0 / PI;
mizuki_akaike 46:a8617076f021 239 double dlat = abs(pointLat - get_dec_latitude()) * d2r;
mizuki_akaike 46:a8617076f021 240 double dlong = abs(pontLong - get_dec_longitude()) * d2r;
mizuki_akaike 46:a8617076f021 241 double y = sin(dlong) * cos(pointLat * d2r);
mizuki_akaike 46:a8617076f021 242 double x = cos(get_dec_latitude()*d2r)*sin(pointLat*d2r) - sin(get_dec_latitude()*d2r)*cos(pointLat*d2r)*cos(dlong);
mizuki_akaike 46:a8617076f021 243 return atan2(y,x)*r2d;
mizuki_akaike 46:a8617076f021 244 }
mizuki_akaike 46:a8617076f021 245
mizuki_akaike 46:a8617076f021 246 /*
mizuki_akaike 46:a8617076f021 247 var y = Math.sin(dLon) * Math.cos(lat2);
mizuki_akaike 46:a8617076f021 248 var x = Math.cos(lat1)*Math.sin(lat2) -
mizuki_akaike 46:a8617076f021 249 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
mizuki_akaike 46:a8617076f021 250 var brng = Math.atan2(y, x).toDeg();
mizuki_akaike 46:a8617076f021 251 */
mizuki_akaike 46:a8617076f021 252
mizuki_akaike 46:a8617076f021 253 /*
mizuki_akaike 46:a8617076f021 254 The Haversine formula according to Dr. Math.
mizuki_akaike 46:a8617076f021 255 http://mathforum.org/library/drmath/view/51879.html
mizuki_akaike 46:a8617076f021 256
mizuki_akaike 46:a8617076f021 257 dlon = lon2 - lon1
mizuki_akaike 46:a8617076f021 258 dlat = lat2 - lat1
mizuki_akaike 46:a8617076f021 259 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2
mizuki_akaike 46:a8617076f021 260 c = 2 * atan2(sqrt(a), sqrt(1-a))
mizuki_akaike 46:a8617076f021 261 d = R * c
mizuki_akaike 46:a8617076f021 262
mizuki_akaike 46:a8617076f021 263 Where
mizuki_akaike 46:a8617076f021 264 * dlon is the change in longitude
mizuki_akaike 46:a8617076f021 265 * dlat is the change in latitude
mizuki_akaike 46:a8617076f021 266 * c is the great circle distance in Radians.
mizuki_akaike 46:a8617076f021 267 * R is the radius of a spherical Earth.
mizuki_akaike 46:a8617076f021 268 * The locations of the two points in
mizuki_akaike 46:a8617076f021 269 spherical coordinates (longitude and
mizuki_akaike 46:a8617076f021 270 latitude) are lon1,lat1 and lon2, lat2.
mizuki_akaike 46:a8617076f021 271 */
mizuki_akaike 46:a8617076f021 272 double GPS::calc_dist_to_mi(float pointLat, float pontLong) {
mizuki_akaike 46:a8617076f021 273 const double d2r = PI / 180.0;
mizuki_akaike 46:a8617076f021 274 double dlat = pointLat - get_dec_latitude();
mizuki_akaike 46:a8617076f021 275 double dlong = pontLong - get_dec_longitude();
mizuki_akaike 46:a8617076f021 276 double a = pow(sin(dlat/2.0),2.0) + cos(get_dec_latitude()*d2r) * cos(pointLat*d2r) * pow(sin(dlong/2.0),2.0);
mizuki_akaike 46:a8617076f021 277 double c = 2.0 * asin(sqrt(abs(a)));
mizuki_akaike 46:a8617076f021 278 double d = 63.765 * c;
mizuki_akaike 46:a8617076f021 279
mizuki_akaike 46:a8617076f021 280 return d;
mizuki_akaike 46:a8617076f021 281 }
mizuki_akaike 46:a8617076f021 282
mizuki_akaike 46:a8617076f021 283 double GPS::calc_dist_to_ft(float pointLat, float pontLong) {
mizuki_akaike 46:a8617076f021 284 return calc_dist_to_mi(pointLat, pontLong)*5280.0;
mizuki_akaike 46:a8617076f021 285 }
mizuki_akaike 46:a8617076f021 286
mizuki_akaike 46:a8617076f021 287 double GPS::calc_dist_to_km(float pointLat, float pontLong) {
mizuki_akaike 46:a8617076f021 288 return calc_dist_to_mi(pointLat, pontLong)*1.609344;
mizuki_akaike 46:a8617076f021 289 }
mizuki_akaike 46:a8617076f021 290
mizuki_akaike 46:a8617076f021 291 double GPS::calc_dist_to_m(float pointLat, float pontLong) {
mizuki_akaike 46:a8617076f021 292 return calc_dist_to_mi(pointLat, pontLong)*1609.344;
mizuki_akaike 46:a8617076f021 293 }
mizuki_akaike 46:a8617076f021 294
mizuki_akaike 46:a8617076f021 295