Nikola Milenic / GPSM

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

GPSM.cpp@2:32c97cf5c676, 2018-12-04 (annotated)

Committer:
milenicnikola
Date:
Tue Dec 04 16:19:41 2018 +0000
Revision:
2:32c97cf5c676
Parent:
1:5e73e764616c
Child:
3:467dee159d16
Optimized GPS code, increased refresh rate to 5Hz

Who changed what in which revision?

UserRevisionLine numberNew contents of line
milenicnikola 1:5e73e764616c 1 #include "GPSM.h"
milenicnikola 2:32c97cf5c676 2 int GPSM::lock = 0;
milenicnikola 2:32c97cf5c676 3 int GPSM::date = 0;
milenicnikola 2:32c97cf5c676 4 int GPSM::msg_ind = -1;
milenicnikola 2:32c97cf5c676 5 float GPSM::dec_latitude = 0.0;
milenicnikola 2:32c97cf5c676 6 float GPSM::dec_longitude = 0.0;
milenicnikola 2:32c97cf5c676 7 float GPSM::nmea_latitude = 0.0;
milenicnikola 2:32c97cf5c676 8 float GPSM::nmea_longitude = 0.0;
milenicnikola 2:32c97cf5c676 9 float GPSM::speed_km = 0.0;
milenicnikola 2:32c97cf5c676 10 float GPSM::course_d = 0.0;
milenicnikola 2:32c97cf5c676 11 float GPSM::utc_time = 0.0;
milenicnikola 2:32c97cf5c676 12 char GPSM::msg[128] = {};
milenicnikola 2:32c97cf5c676 13 char GPSM::bfr[1030] = {};
milenicnikola 2:32c97cf5c676 14 char GPSM::rmc_status = 'V';
milenicnikola 2:32c97cf5c676 15 char GPSM::ns = ' ';
milenicnikola 2:32c97cf5c676 16 char GPSM::ew = ' ';
milenicnikola 2:32c97cf5c676 17 Serial* GPSM::_gps = 0;
milenicnikola 2:32c97cf5c676 18 bool GPSM::ready = false;
milenicnikola 1:5e73e764616c 19 #ifdef OPEN_LOG
milenicnikola 2:32c97cf5c676 20 bool GPSM::is_logging = false;
milenicnikola 2:32c97cf5c676 21 Logger* _openLog = 0;
milenicnikola 1:5e73e764616c 22 #endif
milenicnikola 2:32c97cf5c676 23 void GPSM::GPSM_Init(PinName tx, PinName rx)
milenicnikola 2:32c97cf5c676 24 {
milenicnikola 2:32c97cf5c676 25
milenicnikola 2:32c97cf5c676 26 _gps=new Serial(tx, rx, 9600);
milenicnikola 2:32c97cf5c676 27
milenicnikola 1:5e73e764616c 28 nmea_longitude = 0.0;
milenicnikola 1:5e73e764616c 29 nmea_latitude = 0.0;
milenicnikola 1:5e73e764616c 30 utc_time = 0;
milenicnikola 1:5e73e764616c 31 ns = ' ';
milenicnikola 1:5e73e764616c 32 ew = ' ';
milenicnikola 1:5e73e764616c 33 lock = 0;
milenicnikola 2:32c97cf5c676 34
milenicnikola 1:5e73e764616c 35 rmc_status = 'V';
milenicnikola 1:5e73e764616c 36 speed_km = 0.0;
milenicnikola 1:5e73e764616c 37 course_d = 0.0;
milenicnikola 1:5e73e764616c 38 date = 0;
milenicnikola 1:5e73e764616c 39
milenicnikola 1:5e73e764616c 40 dec_longitude = 0.0;
milenicnikola 1:5e73e764616c 41 dec_latitude = 0.0;
milenicnikola 1:5e73e764616c 42 #ifdef OPEN_LOG
milenicnikola 1:5e73e764616c 43 is_logging = false;
milenicnikola 1:5e73e764616c 44 _openLog=new Logger()
milenicnikola 1:5e73e764616c 45 #endif
milenicnikola 1:5e73e764616c 46 init_GPS();
milenicnikola 1:5e73e764616c 47 wait(1.0);
milenicnikola 2:32c97cf5c676 48 _gps->attach(callback(GPSM::rx_interrupt));
milenicnikola 1:5e73e764616c 49 }
milenicnikola 2:32c97cf5c676 50 void GPSM::rx_interrupt()
milenicnikola 2:32c97cf5c676 51 {
milenicnikola 2:32c97cf5c676 52 if(msg_ind < 0) {
milenicnikola 2:32c97cf5c676 53 if(_gps->getc() == '$') msg_ind = 0;
milenicnikola 1:5e73e764616c 54 return;
milenicnikola 1:5e73e764616c 55 }
milenicnikola 2:32c97cf5c676 56 msg[msg_ind] = _gps->getc();
milenicnikola 2:32c97cf5c676 57 if (msg[msg_ind] == '\r' || msg_ind==90) {
milenicnikola 2:32c97cf5c676 58 msg[msg_ind] = 0;
milenicnikola 2:32c97cf5c676 59 if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%c,%f,%f,%d", &utc_time, &rmc_status, &nmea_latitude, &ns, &nmea_longitude, &ew, &speed_km, &course_d, &date) >= 2) {
milenicnikola 2:32c97cf5c676 60 if(rmc_status == 'V') lock=0;
milenicnikola 2:32c97cf5c676 61 if(rmc_status == 'A') lock=1;
milenicnikola 2:32c97cf5c676 62 speed_km*=1.852;
milenicnikola 2:32c97cf5c676 63 dec_latitude = nmea_to_dec(nmea_latitude, ns);
milenicnikola 2:32c97cf5c676 64 dec_longitude = nmea_to_dec(nmea_longitude, ew);
milenicnikola 2:32c97cf5c676 65 ready=true;
milenicnikola 2:32c97cf5c676 66 }
milenicnikola 2:32c97cf5c676 67 msg_ind=-1;
milenicnikola 1:5e73e764616c 68 return;
milenicnikola 1:5e73e764616c 69 }
milenicnikola 2:32c97cf5c676 70 msg_ind++;
milenicnikola 1:5e73e764616c 71 }
milenicnikola 1:5e73e764616c 72
milenicnikola 2:32c97cf5c676 73 void GPSM::init_GPS()
milenicnikola 2:32c97cf5c676 74 {
milenicnikola 2:32c97cf5c676 75 //GGA OFF
milenicnikola 2:32c97cf5c676 76 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 77 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 78 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 79 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 80 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 81 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 82 _gps->putc(0xF0);
milenicnikola 2:32c97cf5c676 83 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 84 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 85 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 86 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 87 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 88 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 89 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 90 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 91 _gps->putc(0x24);
milenicnikola 2:32c97cf5c676 92 //GLL OFF
milenicnikola 2:32c97cf5c676 93 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 94 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 95 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 96 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 97 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 98 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 99 _gps->putc(0xF0);
milenicnikola 2:32c97cf5c676 100 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 101 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 102 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 103 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 104 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 105 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 106 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 107 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 108 _gps->putc(0x2B);
milenicnikola 2:32c97cf5c676 109 //GSA OFF
milenicnikola 2:32c97cf5c676 110 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 111 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 112 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 113 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 114 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 115 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 116 _gps->putc(0xF0);
milenicnikola 2:32c97cf5c676 117 _gps->putc(0x02);
milenicnikola 2:32c97cf5c676 118 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 119 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 120 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 121 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 122 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 123 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 124 _gps->putc(0x02);
milenicnikola 2:32c97cf5c676 125 _gps->putc(0x32);
milenicnikola 2:32c97cf5c676 126 //GSV OFF
milenicnikola 2:32c97cf5c676 127 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 128 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 129 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 130 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 131 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 132 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 133 _gps->putc(0xF0);
milenicnikola 2:32c97cf5c676 134 _gps->putc(0x03);
milenicnikola 2:32c97cf5c676 135 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 136 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 137 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 138 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 139 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 140 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 141 _gps->putc(0x03);
milenicnikola 2:32c97cf5c676 142 _gps->putc(0x39);
milenicnikola 2:32c97cf5c676 143 //VTG OFF
milenicnikola 2:32c97cf5c676 144 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 145 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 146 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 147 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 148 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 149 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 150 _gps->putc(0xF0);
milenicnikola 2:32c97cf5c676 151 _gps->putc(0x05);
milenicnikola 2:32c97cf5c676 152 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 153 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 154 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 155 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 156 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 157 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 158 _gps->putc(0x05);
milenicnikola 2:32c97cf5c676 159 _gps->putc(0x47);
milenicnikola 2:32c97cf5c676 160 //RATE 5Hz
milenicnikola 2:32c97cf5c676 161 _gps->putc(0xB5);
milenicnikola 2:32c97cf5c676 162 _gps->putc(0x62);
milenicnikola 2:32c97cf5c676 163 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 164 _gps->putc(0x08);
milenicnikola 2:32c97cf5c676 165 _gps->putc(0x06);
milenicnikola 2:32c97cf5c676 166 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 167 _gps->putc(0xC8);
milenicnikola 2:32c97cf5c676 168 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 169 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 170 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 171 _gps->putc(0x01);
milenicnikola 2:32c97cf5c676 172 _gps->putc(0x00);
milenicnikola 2:32c97cf5c676 173 _gps->putc(0xDE);
milenicnikola 2:32c97cf5c676 174 _gps->putc(0x6A);
milenicnikola 1:5e73e764616c 175 }
milenicnikola 1:5e73e764616c 176
milenicnikola 1:5e73e764616c 177 #ifdef OPEN_LOG
milenicnikola 2:32c97cf5c676 178 void GPSM::start_log()
milenicnikola 2:32c97cf5c676 179 {
milenicnikola 1:5e73e764616c 180 is_logging = true;
milenicnikola 1:5e73e764616c 181 }
milenicnikola 1:5e73e764616c 182
milenicnikola 2:32c97cf5c676 183 void GPSM::new_file(void)
milenicnikola 2:32c97cf5c676 184 {
milenicnikola 1:5e73e764616c 185 _openLog->newFile();
milenicnikola 1:5e73e764616c 186 }
milenicnikola 1:5e73e764616c 187
milenicnikola 2:32c97cf5c676 188 void GPSM::stop_log(void)
milenicnikola 2:32c97cf5c676 189 {
milenicnikola 1:5e73e764616c 190 is_logging = false;
milenicnikola 1:5e73e764616c 191 }
milenicnikola 1:5e73e764616c 192 #endif
milenicnikola 1:5e73e764616c 193
milenicnikola 2:32c97cf5c676 194 float GPSM::nmea_to_dec(float deg_coord, char nsew)
milenicnikola 2:32c97cf5c676 195 {
milenicnikola 1:5e73e764616c 196 int degree = (int)(deg_coord/100);
milenicnikola 1:5e73e764616c 197 float minutes = deg_coord - degree*100;
milenicnikola 1:5e73e764616c 198 float dec_deg = minutes / 60;
milenicnikola 1:5e73e764616c 199 float decimal = degree + dec_deg;
milenicnikola 1:5e73e764616c 200 if (nsew == 'S' || nsew == 'W') { // return negative
milenicnikola 1:5e73e764616c 201 decimal *= -1;
milenicnikola 1:5e73e764616c 202 }
milenicnikola 1:5e73e764616c 203 return decimal;
milenicnikola 1:5e73e764616c 204 }
milenicnikola 1:5e73e764616c 205
milenicnikola 2:32c97cf5c676 206 int GPSM::poll()
milenicnikola 2:32c97cf5c676 207 {
milenicnikola 2:32c97cf5c676 208 if (!ready) return NOT_READY;
milenicnikola 1:5e73e764616c 209 #ifdef OPEN_LOG
milenicnikola 2:32c97cf5c676 210 if (is_logging && lock) {
milenicnikola 2:32c97cf5c676 211 format_for_log();
milenicnikola 2:32c97cf5c676 212 _openLog->write(bfr);
milenicnikola 2:32c97cf5c676 213 }
milenicnikola 1:5e73e764616c 214 #endif
milenicnikola 2:32c97cf5c676 215 ready=false;
milenicnikola 2:32c97cf5c676 216 if (lock) return GOOD;
milenicnikola 2:32c97cf5c676 217 else return NO_LOCK;
milenicnikola 1:5e73e764616c 218 }
milenicnikola 1:5e73e764616c 219
milenicnikola 1:5e73e764616c 220
milenicnikola 1:5e73e764616c 221 // INTERNAL FUNCTINS ////////////////////////////////////////////////////////////
milenicnikola 2:32c97cf5c676 222 float GPSM::trunc(float v)
milenicnikola 2:32c97cf5c676 223 {
milenicnikola 1:5e73e764616c 224 if (v < 0.0) {
milenicnikola 1:5e73e764616c 225 v*= -1.0;
milenicnikola 1:5e73e764616c 226 v = floor(v);
milenicnikola 1:5e73e764616c 227 v*=-1.0;
milenicnikola 1:5e73e764616c 228 } else {
milenicnikola 1:5e73e764616c 229 v = floor(v);
milenicnikola 1:5e73e764616c 230 }
milenicnikola 1:5e73e764616c 231 return v;
milenicnikola 1:5e73e764616c 232 }
milenicnikola 1:5e73e764616c 233
milenicnikola 2:32c97cf5c676 234 void GPSM::format_for_log()
milenicnikola 2:32c97cf5c676 235 {
milenicnikola 1:5e73e764616c 236 bfr[0] = '$';
milenicnikola 1:5e73e764616c 237 for (int i = 0; i < 1022; i++) {
milenicnikola 1:5e73e764616c 238 bfr[i+1] = msg[i];
milenicnikola 1:5e73e764616c 239 if (msg[i] == 0 || msg[i] =='$') {
milenicnikola 1:5e73e764616c 240 bfr[i+1] = '\r';
milenicnikola 1:5e73e764616c 241 bfr[i+2] = '\n';
milenicnikola 1:5e73e764616c 242 bfr[i+3] = 0;
milenicnikola 1:5e73e764616c 243 return;
milenicnikola 1:5e73e764616c 244 }
milenicnikola 1:5e73e764616c 245 }
milenicnikola 1:5e73e764616c 246 error("Overflow in format");
milenicnikola 1:5e73e764616c 247 }
milenicnikola 1:5e73e764616c 248
milenicnikola 1:5e73e764616c 249 // GET FUNCTIONS /////////////////////////////////////////////////////////////////
milenicnikola 2:32c97cf5c676 250 float GPSM::get_nmea_longitude()
milenicnikola 2:32c97cf5c676 251 {
milenicnikola 2:32c97cf5c676 252 return nmea_longitude;
milenicnikola 1:5e73e764616c 253 }
milenicnikola 1:5e73e764616c 254
milenicnikola 2:32c97cf5c676 255 float GPSM::get_dec_longitude()
milenicnikola 2:32c97cf5c676 256 {
milenicnikola 2:32c97cf5c676 257 return dec_longitude;
milenicnikola 1:5e73e764616c 258 }
milenicnikola 1:5e73e764616c 259
milenicnikola 2:32c97cf5c676 260 float GPSM::get_nmea_latitude()
milenicnikola 2:32c97cf5c676 261 {
milenicnikola 2:32c97cf5c676 262 return nmea_latitude;
milenicnikola 1:5e73e764616c 263 }
milenicnikola 1:5e73e764616c 264
milenicnikola 2:32c97cf5c676 265 float GPSM::get_dec_latitude()
milenicnikola 2:32c97cf5c676 266 {
milenicnikola 2:32c97cf5c676 267 return dec_latitude;
milenicnikola 1:5e73e764616c 268 }
milenicnikola 1:5e73e764616c 269
milenicnikola 2:32c97cf5c676 270 float GPSM::get_course_t()
milenicnikola 2:32c97cf5c676 271 {
milenicnikola 2:32c97cf5c676 272 return course_d;
milenicnikola 1:5e73e764616c 273 }
milenicnikola 1:5e73e764616c 274
milenicnikola 2:32c97cf5c676 275 float GPSM::get_speed_km()
milenicnikola 2:32c97cf5c676 276 {
milenicnikola 2:32c97cf5c676 277 return speed_km;
milenicnikola 1:5e73e764616c 278 }