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@3:467dee159d16, 2018-12-13 (annotated)

Committer:
milenicnikola
Date:
Thu Dec 13 20:29:18 2018 +0000
Revision:
3:467dee159d16
Parent:
2:32c97cf5c676
Child:
4:61f6f5ae0f8c
Published

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