IGOR / Mbed 2 deprecated PONY_Ph0-uAXIS

whatever

Dependencies:   C027 C027_Support M2XStreamClient PowerControl jsonlite mbed-rtos mbed

Fork of PONY_Ph0-uAXIS by Sean McBeath

PONY_Loc.cpp@54:6ce53a145fa0, 2016-01-20 (annotated)

Committer:
sgmcb
Date:
Wed Jan 20 21:51:25 2016 +0000
Revision:
54:6ce53a145fa0
Parent:
41:f603d76dc6fe 
fuck you

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sgmcb 38:d86e744166b6 1 // PONY_Loc.cpp
sgmcb 38:d86e744166b6 2 // Location code for PONY
sgmcb 38:d86e744166b6 3 // (C) 2015 Igor Institute
sgmcb 38:d86e744166b6 4
sgmcb 38:d86e744166b6 5 #include "PONY_Loc.h"
sgmcb 38:d86e744166b6 6
sgmcb 38:d86e744166b6 7 float kGPSloopPeriod = 0.25; // Globally set our GPS read period
sgmcb 38:d86e744166b6 8
sgmcb 38:d86e744166b6 9
sgmcb 38:d86e744166b6 10
sgmcb 38:d86e744166b6 11 // Define our array of "talkers"
sgmcb 38:d86e744166b6 12 const char* kNMEA_talkers[] = { "", "GSA", "GSV", "RMC", "GLL", "GGA", "GNS", "VTG" };
sgmcb 38:d86e744166b6 13 const int kTalkerCount = 7;
sgmcb 38:d86e744166b6 14
sgmcb 38:d86e744166b6 15 //-----------------------------------------------------------------------------
sgmcb 38:d86e744166b6 16 //-----------------------------------------------------------------------------
sgmcb 38:d86e744166b6 17
sgmcb 38:d86e744166b6 18 int readyGPS(GPSbundle* GPSb, int loopmax) {
sgmcb 38:d86e744166b6 19
sgmcb 38:d86e744166b6 20 for (int loopiter = 0; loopiter < loopmax; loopiter++) {
sgmcb 38:d86e744166b6 21 if( (GPSb->bRet = (GPSb->bGPS).getMessage(GPSb->bBuf, GPSb->bBufLen)) > 0 ) {
sgmcb 38:d86e744166b6 22 printf("GPS ready\r\n");
sgmcb 38:d86e744166b6 23 return 1;
sgmcb 38:d86e744166b6 24 }
sgmcb 38:d86e744166b6 25 wait(1);
sgmcb 38:d86e744166b6 26 }
sgmcb 38:d86e744166b6 27 printf("GPS ready timeout\r\n");
sgmcb 38:d86e744166b6 28 return 0;
sgmcb 38:d86e744166b6 29 }
sgmcb 38:d86e744166b6 30
sgmcb 38:d86e744166b6 31 //-----------------------------------------------------------------------------
sgmcb 38:d86e744166b6 32
sgmcb 38:d86e744166b6 33 int getNMEAtalker (const char* msg) {
sgmcb 38:d86e744166b6 34
sgmcb 38:d86e744166b6 35 // TODO: Need to verify we're receiving NMEA sentences at some point in this chain
sgmcb 38:d86e744166b6 36 #define _CHECK_TALKER(s) ((msg[3] == s[0]) && (msg[4] == s[1]) && (msg[5] == s[2]))
sgmcb 38:d86e744166b6 37
sgmcb 38:d86e744166b6 38 for (int i = 1; i <= kTalkerCount; i++) {
sgmcb 38:d86e744166b6 39 if( _CHECK_TALKER(kNMEA_talkers[i]) )
sgmcb 38:d86e744166b6 40 return i;
sgmcb 38:d86e744166b6 41 }
sgmcb 38:d86e744166b6 42 return 0;
sgmcb 38:d86e744166b6 43 }
sgmcb 38:d86e744166b6 44
sgmcb 38:d86e744166b6 45
sgmcb 38:d86e744166b6 46
sgmcb 38:d86e744166b6 47 //-----------------------------------------------------------------------------
sgmcb 38:d86e744166b6 48
sgmcb 38:d86e744166b6 49 int fetchLocBundle(GPSbundle* GPSb, GPSloc* LOCb, int timeout) {
sgmcb 38:d86e744166b6 50
sgmcb 38:d86e744166b6 51 int locFlag = 0;
sgmcb 38:d86e744166b6 52 int altFlag = 0;
sgmcb 38:d86e744166b6 53 int velFlag = 0;
sgmcb 38:d86e744166b6 54
sgmcb 38:d86e744166b6 55 int loopCountMax = (float) timeout / kGPSloopPeriod;
sgmcb 38:d86e744166b6 56
sgmcb 38:d86e744166b6 57
sgmcb 38:d86e744166b6 58
sgmcb 38:d86e744166b6 59
sgmcb 38:d86e744166b6 60 // Let's get some messages
sgmcb 38:d86e744166b6 61 for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
sgmcb 38:d86e744166b6 62
sgmcb 38:d86e744166b6 63 GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
sgmcb 38:d86e744166b6 64 GPSb->bLen = LENGTH(GPSb->bRet);
sgmcb 38:d86e744166b6 65
sgmcb 38:d86e744166b6 66 // TODO: Verify it's NMEA data
sgmcb 38:d86e744166b6 67
sgmcb 38:d86e744166b6 68
sgmcb 38:d86e744166b6 69 int talker = getNMEAtalker(GPSb->bBuf);
sgmcb 38:d86e744166b6 70
sgmcb 38:d86e744166b6 71 if(talker > 0)
sgmcb 38:d86e744166b6 72 printf("%s talker\r\n", kNMEA_talkers[talker]);
sgmcb 38:d86e744166b6 73 else
sgmcb 38:d86e744166b6 74 printf("Non-LOC talker\r\n");
sgmcb 38:d86e744166b6 75
sgmcb 38:d86e744166b6 76
sgmcb 38:d86e744166b6 77
sgmcb 38:d86e744166b6 78 // Talker switch statement
sgmcb 38:d86e744166b6 79 switch(talker)
sgmcb 38:d86e744166b6 80 {
sgmcb 38:d86e744166b6 81 // GLL talker
sgmcb 38:d86e744166b6 82 case 4:
sgmcb 38:d86e744166b6 83
sgmcb 38:d86e744166b6 84 if(fillLocBundle(GPSb,LOCb))
sgmcb 38:d86e744166b6 85 locFlag = 1;
sgmcb 38:d86e744166b6 86 break;
sgmcb 38:d86e744166b6 87
sgmcb 38:d86e744166b6 88 // GGA talker
sgmcb 38:d86e744166b6 89 case 5:
sgmcb 38:d86e744166b6 90 (GPSb->bGPS).getNmeaAngle(8,GPSb->bBuf,GPSb->bLen,LOCb->alt);
sgmcb 38:d86e744166b6 91 altFlag = 1;
sgmcb 38:d86e744166b6 92 break;
sgmcb 38:d86e744166b6 93
sgmcb 38:d86e744166b6 94 // GNS talker
sgmcb 38:d86e744166b6 95 case 6:
sgmcb 38:d86e744166b6 96 (GPSb->bGPS).getNmeaAngle(9,GPSb->bBuf,GPSb->bLen,LOCb->alt);
sgmcb 38:d86e744166b6 97 break;
sgmcb 38:d86e744166b6 98
sgmcb 38:d86e744166b6 99 // VTG talker
sgmcb 38:d86e744166b6 100 case 7:
sgmcb 38:d86e744166b6 101
sgmcb 38:d86e744166b6 102
sgmcb 38:d86e744166b6 103 velFlag = 1;
sgmcb 38:d86e744166b6 104 break;
sgmcb 38:d86e744166b6 105
sgmcb 38:d86e744166b6 106 default:
sgmcb 38:d86e744166b6 107 printf("Unregistered talker\r\n");
sgmcb 38:d86e744166b6 108
sgmcb 38:d86e744166b6 109 }
sgmcb 38:d86e744166b6 110
sgmcb 38:d86e744166b6 111 if( locFlag && altFlag && velFlag )
sgmcb 38:d86e744166b6 112 return 7;
sgmcb 38:d86e744166b6 113
sgmcb 38:d86e744166b6 114 wait( kGPSloopPeriod );
sgmcb 38:d86e744166b6 115 }
sgmcb 38:d86e744166b6 116 return 0; // False return for timeout
sgmcb 38:d86e744166b6 117
sgmcb 38:d86e744166b6 118
sgmcb 38:d86e744166b6 119 }
sgmcb 38:d86e744166b6 120
sgmcb 38:d86e744166b6 121
sgmcb 38:d86e744166b6 122
sgmcb 38:d86e744166b6 123
sgmcb 38:d86e744166b6 124 int fillLocBundle(GPSbundle* GPSb, GPSloc* LocBundle) {
sgmcb 38:d86e744166b6 125
sgmcb 38:d86e744166b6 126 if ( verifyGLL(GPSb, LocBundle) ) {
sgmcb 38:d86e744166b6 127 printf("GLL verified!\r\n");
sgmcb 38:d86e744166b6 128 (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
sgmcb 38:d86e744166b6 129 (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
sgmcb 38:d86e744166b6 130 (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
sgmcb 38:d86e744166b6 131 return 1;
sgmcb 38:d86e744166b6 132 }
sgmcb 38:d86e744166b6 133 return 0;
sgmcb 38:d86e744166b6 134 }
sgmcb 38:d86e744166b6 135
sgmcb 38:d86e744166b6 136
sgmcb 38:d86e744166b6 137
sgmcb 38:d86e744166b6 138
sgmcb 38:d86e744166b6 139
sgmcb 38:d86e744166b6 140 int fetchLaLo(GPSbundle* GPSb, GPSloc* LocBundle, int timeout) {
sgmcb 38:d86e744166b6 141
sgmcb 38:d86e744166b6 142 int loopCountMax = (float) timeout / kGPSloopPeriod;
sgmcb 38:d86e744166b6 143
sgmcb 38:d86e744166b6 144
sgmcb 38:d86e744166b6 145 for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
sgmcb 38:d86e744166b6 146
sgmcb 38:d86e744166b6 147 GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
sgmcb 38:d86e744166b6 148 GPSb->bLen = LENGTH(GPSb->bRet);
sgmcb 38:d86e744166b6 149
sgmcb 38:d86e744166b6 150
sgmcb 38:d86e744166b6 151 // TODO: Verify it's NMEA data
sgmcb 38:d86e744166b6 152 int talker = getNMEAtalker(GPSb->bBuf);
sgmcb 38:d86e744166b6 153 printf("\r\nTalker=%i",talker);
sgmcb 38:d86e744166b6 154
sgmcb 38:d86e744166b6 155
sgmcb 38:d86e744166b6 156 if( talker == 4 ) { // Make sure that this matches our array indexing
sgmcb 38:d86e744166b6 157 printf("GLL data\r\b");
sgmcb 38:d86e744166b6 158
sgmcb 38:d86e744166b6 159 if ( verifyGLL(GPSb, LocBundle) ) {
sgmcb 38:d86e744166b6 160 printf("GLL verified!\r\n");
sgmcb 38:d86e744166b6 161 (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
sgmcb 38:d86e744166b6 162 (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
sgmcb 38:d86e744166b6 163 (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
sgmcb 38:d86e744166b6 164 return 1;
sgmcb 38:d86e744166b6 165 }
sgmcb 38:d86e744166b6 166 }
sgmcb 38:d86e744166b6 167
sgmcb 38:d86e744166b6 168
sgmcb 38:d86e744166b6 169 wait( kGPSloopPeriod );
sgmcb 38:d86e744166b6 170 }
sgmcb 38:d86e744166b6 171 return 0; // False return for timeout
sgmcb 38:d86e744166b6 172
sgmcb 38:d86e744166b6 173 }
sgmcb 38:d86e744166b6 174
sgmcb 38:d86e744166b6 175
sgmcb 38:d86e744166b6 176
sgmcb 38:d86e744166b6 177
sgmcb 38:d86e744166b6 178
sgmcb 38:d86e744166b6 179
sgmcb 38:d86e744166b6 180
sgmcb 38:d86e744166b6 181
sgmcb 38:d86e744166b6 182 int fetchAlt(GPSbundle* GPSb, GPSloc* LocBundle, int timeout) {
sgmcb 38:d86e744166b6 183
sgmcb 38:d86e744166b6 184 int loopCountMax = (float) timeout / kGPSloopPeriod;
sgmcb 38:d86e744166b6 185
sgmcb 38:d86e744166b6 186
sgmcb 38:d86e744166b6 187 for (int loopCounter = 0; loopCounter < loopCountMax; loopCounter++) {
sgmcb 38:d86e744166b6 188
sgmcb 38:d86e744166b6 189 GPSb->bRet = GPSb->bGPS.getMessage(GPSb->bBuf,GPSb->bBufLen);
sgmcb 38:d86e744166b6 190 GPSb->bLen = LENGTH(GPSb->bRet);
sgmcb 38:d86e744166b6 191
sgmcb 38:d86e744166b6 192
sgmcb 38:d86e744166b6 193 // TODO: Verify it's NMEA data
sgmcb 38:d86e744166b6 194 int talker = getNMEAtalker(GPSb->bBuf);
sgmcb 38:d86e744166b6 195 printf("\r\nTalker=%i",talker);
sgmcb 38:d86e744166b6 196
sgmcb 38:d86e744166b6 197
sgmcb 38:d86e744166b6 198 if( talker == 4 ) { // Make sure that this matches our array indexing
sgmcb 38:d86e744166b6 199 printf("GLL data\r\b");
sgmcb 38:d86e744166b6 200
sgmcb 38:d86e744166b6 201 if ( verifyGLL(GPSb, LocBundle) ) {
sgmcb 38:d86e744166b6 202 printf("GLL verified!\r\n");
sgmcb 38:d86e744166b6 203 (GPSb->bGPS).getNmeaAngle(1,GPSb->bBuf,GPSb->bLen,LocBundle->lat);
sgmcb 38:d86e744166b6 204 (GPSb->bGPS).getNmeaAngle(3,GPSb->bBuf,GPSb->bLen,LocBundle->lon);
sgmcb 38:d86e744166b6 205 (GPSb->bGPS).getNmeaItem(5,GPSb->bBuf,GPSb->bLen,LocBundle->tmstmp);
sgmcb 38:d86e744166b6 206 return 1;
sgmcb 38:d86e744166b6 207 }
sgmcb 38:d86e744166b6 208 }
sgmcb 38:d86e744166b6 209
sgmcb 38:d86e744166b6 210
sgmcb 38:d86e744166b6 211 wait( kGPSloopPeriod );
sgmcb 38:d86e744166b6 212 }
sgmcb 38:d86e744166b6 213 return 0; // False return for timeout
sgmcb 38:d86e744166b6 214
sgmcb 38:d86e744166b6 215 }
sgmcb 38:d86e744166b6 216
sgmcb 38:d86e744166b6 217
sgmcb 38:d86e744166b6 218
sgmcb 38:d86e744166b6 219 //-----------------------------------------------------------------------------
sgmcb 38:d86e744166b6 220
sgmcb 38:d86e744166b6 221 int verifyGLL(GPSbundle* GPSb, GPSloc* LocBundle) {
sgmcb 38:d86e744166b6 222
sgmcb 38:d86e744166b6 223 char ch = '-';
sgmcb 38:d86e744166b6 224
sgmcb 38:d86e744166b6 225 (GPSb->bGPS).getNmeaItem(6,GPSb->bBuf,GPSb->bLen,ch);
sgmcb 38:d86e744166b6 226
sgmcb 38:d86e744166b6 227 printf("\r\nCheck character: %c\r\n",ch);
sgmcb 38:d86e744166b6 228 printf("\r\n%s\r\n", GPSb->bBuf);
sgmcb 38:d86e744166b6 229
sgmcb 38:d86e744166b6 230 if(ch == 'A')
sgmcb 38:d86e744166b6 231 return 1;
sgmcb 38:d86e744166b6 232 else {
sgmcb 38:d86e744166b6 233
sgmcb 38:d86e744166b6 234 return 0;
sgmcb 38:d86e744166b6 235 }
sgmcb 38:d86e744166b6 236 }
sgmcb 38:d86e744166b6 237
sgmcb 38:d86e744166b6 238
sgmcb 38:d86e744166b6 239
sgmcb 38:d86e744166b6 240
sgmcb 39:c14aff678b25 241
sgmcb 39:c14aff678b25 242 void parseM2XLocation(const char* name,
sgmcb 39:c14aff678b25 243 double latitude,
sgmcb 39:c14aff678b25 244 double longitude,
sgmcb 39:c14aff678b25 245 double elevation,
sgmcb 39:c14aff678b25 246 const char* timestamp,
sgmcb 39:c14aff678b25 247 int index,
sgmcb 39:c14aff678b25 248 void* context) {
sgmcb 39:c14aff678b25 249
sgmcb 39:c14aff678b25 250 printf("Found previous M2X location, index: %d\r\n", index);
sgmcb 39:c14aff678b25 251 printf("Name: %s Latitude: %lf Longitude: %lf\r\n", name, latitude, longitude);
sgmcb 39:c14aff678b25 252 printf("Elevation: %lf Timestamp: %s\r\n", elevation, timestamp);
sgmcb 39:c14aff678b25 253
sgmcb 39:c14aff678b25 254 }
sgmcb 39:c14aff678b25 255
sgmcb 39:c14aff678b25 256
sgmcb 39:c14aff678b25 257
sgmcb 39:c14aff678b25 258
sgmcb 39:c14aff678b25 259
sgmcb 41:f603d76dc6fe 260
sgmcb 41:f603d76dc6fe 261
sgmcb 41:f603d76dc6fe 262 void getFormattedTime(time_t* pSysTime, tm* formatTime) {
sgmcb 41:f603d76dc6fe 263
sgmcb 41:f603d76dc6fe 264 formatTime = localtime(pSysTime);
sgmcb 41:f603d76dc6fe 265
sgmcb 41:f603d76dc6fe 266 formatTime->tm_year +=1900; //Add 1900 in because mbed reports year as "since 1900"
sgmcb 41:f603d76dc6fe 267
sgmcb 41:f603d76dc6fe 268 /*
sgmcb 41:f603d76dc6fe 269 printf("\r\n\nSys Time as structure values:\r\n");
sgmcb 41:f603d76dc6fe 270 printf("Year=%i\r\n", formatTime->tm_year);
sgmcb 41:f603d76dc6fe 271 printf("Month=%i\r\n", formatTime->tm_mon);
sgmcb 41:f603d76dc6fe 272 printf("Day=%i\r\n", formatTime->tm_mday);
sgmcb 41:f603d76dc6fe 273 printf("Hour=%i\r\n", formatTime->tm_hour);
sgmcb 41:f603d76dc6fe 274 printf("Minute=%i\r\n", formatTime->tm_min);
sgmcb 41:f603d76dc6fe 275 printf("Second=%i\r\n", formatTime->tm_sec);
sgmcb 41:f603d76dc6fe 276 */
sgmcb 41:f603d76dc6fe 277
sgmcb 41:f603d76dc6fe 278 return;
sgmcb 41:f603d76dc6fe 279 };
sgmcb 41:f603d76dc6fe 280
sgmcb 41:f603d76dc6fe 281