This repository has been archived by the owner on May 26, 2020. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathNeo6MGPS.cpp
88 lines (80 loc) · 3.3 KB
/
Neo6MGPS.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
#include "Neo6MGPS.h"
/*
static const unsigned char UBLOX_INIT[] PROGMEM = {
// Disable specific NMEA sentences
//0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x24, // GxGGA off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x01,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x2B, // GxGLL off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x02,0x00,0x00,0x00,0x00,0x00,0x01,0x02,0x32, // GxGSA off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x03,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x39, // GxGSV off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x04,0x00,0x00,0x00,0x00,0x00,0x01,0x04,0x40, // GxRMC off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x05,0x00,0x00,0x00,0x00,0x00,0x01,0x05,0x47, // GxVTG off
0xB5,0x62,0x06,0x01,0x08,0x00,0xF0,0x08,0x00,0x00,0x00,0x00,0x00,0x01,0x08,0x5C, // GxZDA off
// Rate (pick one)
//0xB5,0x62,0x06,0x08,0x06,0x00,0x64,0x00,0x01,0x00,0x01,0x00,0x7A,0x12, //(10Hz)
0xB5,0x62,0x06,0x08,0x06,0x00,0xC8,0x00,0x01,0x00,0x01,0x00,0xDE,0x6A, //(5Hz)
//0xB5,0x62,0x06,0x08,0x06,0x00,0xE8,0x03,0x01,0x00,0x01,0x00,0x01,0x39, //(1Hz)
// Save Configuration to persistent memory
0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0x31, 0xBF,
};
*/
// latitude & longitude in degrees into x,y
// coordinates in millimeters on the surface of earth
#ifndef STM32_CORE_VERSION
void lat_lon_to_x_y_mm(double lat, double lon, int64_t &x, int64_t &y)
{
double lat_rad = radians(lat);
x = static_cast<int64_t>( 111132952.548 * lat );
y = static_cast<int64_t>( (((111412.84 * cos(lat_rad) - 93.5 * cos(3.0 * lat_rad)) + 0.118 * cos(5.0 * lat_rad)) * lon) * 1000.0 );
}
#else
void lat_lon_to_x_y_m(double lat, double lon, double &x, double &y)
{
double lat_rad = radians(lat);
x = 111132.9525477777777777777777777 * lat;
y = ((111412.84 * cos(lat_rad) - 93.5 * cos(3.0 * lat_rad)) + 0.118 * cos(5.0 * lat_rad)) * lon;
}
#endif
Neo6MGPS::Neo6MGPS(int GPS_TX_PIN, int GPS_RX_PIN)
: ss{ GPS_TX_PIN, GPS_RX_PIN }
{
}
void Neo6MGPS::begin(uint16_t GPS_BAUD_RATE)
{
ss.begin(GPS_BAUD_RATE);
/*
// configure the ublox gps module
for (size_t i = 0; i < sizeof(UBLOX_INIT); i++) {
ss.write( pgm_read_byte(UBLOX_INIT+i) );
};
*/
}
// This function attempts to read GPS module stream and search for 4 GPS satellites.
// If not possible it returns false, otherwise it updates lat-lon-alt and returns true.
#ifndef STM32_CORE_VERSION
bool Neo6MGPS::try_read_gps(int64_t& lat_mm, int64_t& lon_mm, int64_t& alt_mm, uint8_t num_gps)
{
while (ss.available() > 0){
gps.encode(ss.read());
if (gps.satellites.isUpdated() && gps.satellites.value() >= num_gps && gps.location.isUpdated()){
lat_lon_to_x_y_mm(gps.location.lat(), gps.location.lng(), lat_mm, lon_mm);
alt_mm = gps.altitude.value() * 10;
return true;
}
}
return false;
}
#else
bool Neo6MGPS::try_read_gps(double& lat_m, double& lon_m, double& alt_m, uint8_t num_gps)
{
while (ss.available() > 0){
gps.encode(ss.read());
if (gps.satellites.isUpdated() && gps.satellites.value() >= num_gps && gps.location.isUpdated()){
lat_lon_to_x_y_m(gps.location.lat(), gps.location.lng(), lat_m, lon_m);
alt_m = gps.altitude.meters();
return true;
}
}
return false;
}
#endif