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@1:5e73e764616c, 2018-12-03 (annotated)

Committer:
milenicnikola
Date:
Mon Dec 03 21:19:16 2018 +0000
Revision:
1:5e73e764616c
Parent:
0:68d3dfe94417
Child:
2:32c97cf5c676
Working GPS

Who changed what in which revision?

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