· 6 years ago · Dec 05, 2019, 05:08 AM
1/************************************************************/
2/* NAME: */
3/* ORGN: MIT */
4/* FILE: GPSd.cpp */
5/* DATE: */
6/************************************************************/
7
8#include <iterator>
9#include "MBUtils.h"
10#include "ACTable.h"
11#include "GPSd.h"
12#include <iostream>
13#include "libgpsmm.h"
14#include <thread>
15#include <ctime>
16#include <chrono>
17#include <cmath>
18
19using namespace std;
20using namespace std::chrono;
21
22//---------------------------------------------------------
23// Constructor
24
25GPSd::GPSd() {
26 p_gpsd_receiver = NULL;
27 m_gpsd_host = "localhost";
28 m_gpsd_port = DEFAULT_GPSD_PORT;
29 gps_data_t *resp = NULL;
30 //m_buf.str() = "";
31 m_json_output = "";
32 m_mag_model = "emm2015";
33 m_gps_mode = 0;
34 m_gps_alt = 0;
35 m_gps_lon = 0;
36 m_gps_alt = 0;
37 m_gps_spd = 0;
38 m_gps_trk = 0;
39}
40
41//---------------------------------------------------------
42// Destructor
43
44GPSd::~GPSd()
45{
46 if (p_gpsd_receiver) delete p_gpsd_receiver;
47 if (p_gpsdata) delete p_gpsdata;
48}
49
50//---------------------------------------------------------
51// Procedure: OnNewMail
52
53bool GPSd::OnNewMail(MOOSMSG_LIST &NewMail)
54{
55 AppCastingMOOSApp::OnNewMail(NewMail);
56
57 MOOSMSG_LIST::iterator p;
58 for(p=NewMail.begin(); p!=NewMail.end(); p++) {
59 CMOOSMsg &msg = *p;
60 string key = msg.GetKey();
61 if(key != "APPCAST_REQ") {// handled by AppCastingMOOSApp
62 reportRunWarning("Unhandled Mail: " + key);
63 }
64 }
65
66 return(true);
67}
68
69//---------------------------------------------------------
70// Procedure: OnConnectToServer
71
72bool GPSd::OnConnectToServer()
73{
74 registerVariables();
75 return(true);
76}
77
78//---------------------------------------------------------
79// Procedure: Iterate()
80// happens AppTick times per second
81#if GPSD_API_MAJOR_VERSION >= 4
82#define SATS_VISIBLE p->satellites_visible
83#elif GPSD_API_MAJOR_VERSION == 3
84#define SATS_VISIBLE p->satellites
85#else
86#error "gpsd_client only supports gpsd API versions 3+"
87#endif
88
89bool GPSd::Iterate()
90{
91 AppCastingMOOSApp::Iterate();
92#if GPSD_API_MAJOR_VERSION >= 5
93 if (!gps->waiting(1e6))
94 return;
95
96 gps_data_t p_gpsdata = gps->read();
97#else
98 gps_data_t p_gpsdata = gps->poll();
99#endif
100
101 //m_buf << p_gpsd_receiver->data(); // grab the data buffer
102 //cerr << "**********************************************************" << endl;
103 //cerr << "Got buffer: " << endl;
104 //cerr << "**********************************************************" << endl;
105 //cerr << m_buf.str() << endl;
106 //cerr << p_gpsd_receiver->data() << endl;
107 //cerr << "**********************************************************" << endl;
108 if ((p_gpsdata != NULL) && (p_gpsdata->set) && (p_gpsdata->status & STATUS_FIX)) {
109 m_gps_mode = p_gpsdata->fix.mode;
110 m_gps_time = p_gpsdata->fix.time
111 m_gps_lat = p_gpsdata->fix.latitude;
112 m_gps_lon = p_gpsdata->fix.longitude;
113 m_gps_alt = p_gpsdata->fix.altitude ;
114 m_gps_spd = p_gpsdata->fix.speed;
115 m_gps_trk = p_gpsdata->fix.track
116
117 Notify("GPSD_mode", m_gps_mode);
118 Notify("GPSD_latitude", m_gps_lat);
119 Notify("GPSD_longitude", m_gps_lon);
120 Notify("GPSD_elevation", m_gps_alt);
121 Notify("GPSD_speed", m_gps_spd);
122 Notify("GPSD_track", m_gps_trk);
123 m_json_output = p_gpsd_receiver->data();
124 Notify("GPSD_json", m_json_output);
125 }
126
127 p_gpsd_receiver->clear_fix();
128
129 AppCastingMOOSApp::PostReport();
130 return(true);
131}
132
133//---------------------------------------------------------
134// Procedure: OnStartUp()
135// happens before connection is open
136
137bool GPSd::OnStartUp()
138{
139 AppCastingMOOSApp::OnStartUp();
140
141 STRING_LIST sParams;
142 m_MissionReader.EnableVerbatimQuoting(false);
143 if(!m_MissionReader.GetConfiguration(GetAppName(), sParams))
144 reportConfigWarning("No config block found for " + GetAppName());
145
146 STRING_LIST::iterator p;
147 for(p=sParams.begin(); p!=sParams.end(); p++) {
148 string orig = *p;
149 string line = *p;
150 string param = toupper(biteStringX(line, '='));
151 string value = line;
152
153 bool handled = false;
154 if (param == "HOST") {
155 this->m_gpsd_host = value;
156 handled = true;
157 } else if (param == "PORT") {
158 this->m_gpsd_port = value;
159 handled = true;
160 } else if (param == "MAG_MODEL") {
161 this->m_mag_model = value;
162 handled = true;
163 }
164 if(!handled)
165 reportUnhandledConfigWarning(orig);
166
167 }
168#if GPSD_API_MAJOR_VERSION >= 5
169 gps = new gpsmm(m_gpsd_host.c_str(), m_gpsd_port);
170 resp = gps->stream(WATCH_ENABLE);
171#elif GPSD_API_MAJOR_VERSION == 4
172 gps = new gpsmm();
173 gps->open(m_gpsd_host.c_str(), m_gpsd_port);
174 resp = gps->stream(WATCH_ENABLE);
175#else
176 gps = new gpsmm();
177 resp = gps->open(m_gpsd_host.c_str(), m_gpsd_port);
178 gps->query("w\n");
179#endif
180
181 registerVariables();
182 return(true);
183}
184
185//---------------------------------------------------------
186// Procedure: registerVariables
187
188void GPSd::registerVariables()
189{
190 AppCastingMOOSApp::RegisterVariables();
191 // Register("FOOBAR", 0);
192}
193
194
195//------------------------------------------------------------
196// Procedure: buildReport()
197
198bool GPSd::buildReport()
199{
200 m_msgs << "============================================ \n";
201 m_msgs << "File: GPSd.cpp \n";
202 m_msgs << "============================================ \n";
203
204 ACTable actab(7);
205 actab << "Mode | Lat | Lon | Alt | Speed | Track | Mag Declination";
206 actab.addHeaderLines();
207 actab << to_string(m_gps_mode);
208 actab << to_string(m_gps_lat);
209 actab << to_string(m_gps_lon);
210 actab << to_string(m_gps_alt);
211 actab << to_string(m_gps_spd);
212 actab << to_string(m_gps_trk);
213 actab << to_string(m_mag_declination);
214 m_msgs << actab.getFormattedString() << endl;
215 m_msgs << "============================================ \n";
216 m_msgs << "Last JSON: " << m_json_output << endl;;
217
218 return(true);
219}