1 #include "gps.h" 2 decode_gps(uint8_t * data,coordinate * result)3bool decode_gps(uint8_t* data, coordinate* result) { 4 if ((data[0] & 0x0F) > 9) return false; 5 if ((data[1] & 0x0F) > 9) return false; 6 if ((data[2] & 0x0F) > 9) return false; 7 if ((data[3] & 0x0F) > 9) return false; 8 if ((data[4] & 0x0F) > 9) return false; 9 if ((data[5] & 0x0F) > 9) return false; 10 11 float lat = (data[0] & 0x0F) * 10 + 12 (data[1] & 0x0F) + 13 (float) (data[2] & 0x0F) / 6 + 14 (float) (data[3] & 0x0F) / 60 + 15 (float) (data[4] & 0x0F) / 600 + 16 (float) (data[5] & 0x0F) / 6000; 17 18 uint8_t direction = (data[3] & 0xF0); 19 if (direction == 0x50) { 20 // northern hemisphere. nothing to do here, so pass 21 } else if (direction == 0x30) { 22 // southern hemisphere 23 lat *= -1; 24 } else { 25 return false; 26 } 27 28 float lon; 29 30 uint8_t b = data[4] & 0xF0; 31 uint8_t c = data[6]; 32 if (b == 0x50) { 33 if (c >= 0x76 && c < 0x7f) { 34 lon = c - 0x76; 35 } else if (c >= 0x6c && c < 0x75) { 36 lon = 100 + (c - 0x6c); 37 } else if (c >= 0x26 && c < 0x6b) { 38 lon = 110 + (c - 0x26); 39 } else { 40 return false; 41 } 42 } else if (b == 0x30) { 43 if (c >= 0x26 && c < 0x7f) { 44 lon = 10 + (c - 0x26); 45 } else { 46 return false; 47 } 48 } 49 50 b = data[7]; 51 if (b > 0x58 && b <= 0x61) { 52 lon += (float) (b - 0x58) / 60; 53 } else if (b >= 0x26 && b <= 0x57) { 54 lon += (float) (10 + (b - 0x26)) / 60; 55 } else { 56 return false; 57 } 58 59 b = data[8]; 60 if (b >= 0x1c && b < 0x7f) { 61 lon += (float) (b - 0x1c) / 6000; 62 } else { 63 return false; 64 } 65 66 direction = (data[5] & 0xF0); 67 if (direction == 0x50) { 68 // western hemisphere 69 lon *= -1; 70 } else if (direction == 0x30) { 71 // eastern hemisphere. nothing to do here, so pass 72 } else { 73 return false; 74 } 75 76 77 78 if (lat > 90 || lat < -90) return false; 79 if (lon > 180 || lon < -180) return false; 80 81 result->lat = lat; 82 result->lon = lon; 83 84 return true; 85 } 86