Debian Codegraph @

libs/hwdrivers/src/CRaePID.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CRaePID.h>
13 #include <mrpt/system/datetime.h>
14
15 #include <chrono>
16 #include <iostream>
17 #include <iterator>
18 #include <memory>
19 #include <sstream>
20 #include <thread>
21
22 using namespace std;
23 using namespace mrpt::hwdrivers;
24
25 IMPLEMENTS_GENERIC_SENSOR(CRaePID, mrpt::hwdrivers)
26
27 /* -----------------------------------------------------
28 Constructor
29 ----------------------------------------------------- */
30 CRaePID::CRaePID() { m_sensorLabel = "RAE_PID"; }
31 /* -----------------------------------------------------
32 loadConfig_sensorSpecific
33 ----------------------------------------------------- */
34 void CRaePID::loadConfig_sensorSpecific(
35 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
36 {
37 #ifdef _WIN32
38 com_port = configSource.read_string(iniSection, "COM_port_PID", "COM1", true);
39 #else
40 com_port = configSource.read_string(iniSection, "COM_port_PID", "/dev/tty0", true);
41 #endif
42
43 com_bauds = configSource.read_int(iniSection, "baudRate", 9600, false);
44
45 pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
46 pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
47 pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
48 pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
49 pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
50 pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
51 }
52
53 /* -----------------------------------------------------
54 tryToOpenTheCOM
55 ----------------------------------------------------- */
56 bool CRaePID::tryToOpenTheCOM()
57 {
58 if (COM.isOpen()) return true; // Already open
59
60 if (m_verbose) cout << "[CRaePID] Opening " << com_port << " @ " << com_bauds << endl;
61
62 try
63 {
64 COM.open(com_port);
65 // Config:
66 COM.setConfig(com_bauds, 0, 8, 1);
67 // COM.setTimeouts( 1, 0, 1, 1, 1 );
68 COM.setTimeouts(50, 1, 100, 1, 20);
69 // std::this_thread::sleep_for(10ms);
70 COM.purgeBuffers();
71 // std::this_thread::sleep_for(10ms);
72
73 return true; // All OK!
74 }
75 catch (const std::exception& e)
76 {
77 std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring "
78 "the serial port:"
79 << std::endl
80 << e.what();
81 COM.close();
82 return false;
83 }
84 catch (...)
85 {
86 std::cerr << "[CRaePID::tryToOpenTheCOM] Error opening or configuring "
87 "the serial port."
88 << std::endl;
89 COM.close();
90 return false;
91 }
92 }
93
94 /* -----------------------------------------------------
95 doProcess
96 ----------------------------------------------------- */
97 void CRaePID::doProcess()
98 {
99 // Is the COM open?
100 if (!tryToOpenTheCOM())
101 {
102 m_state = ssError;
103 THROW_EXCEPTION("Cannot open the serial port");
104 }
105
106 bool have_reading = false;
107 std::string power_reading;
108 bool time_out = false;
109
110 while (!have_reading)
111 {
112 // Send command to PID to request a measurement
113 COM.purgeBuffers();
114 COM.Write("R", 1);
115
116 // Read PID response
117 power_reading = COM.ReadString(500, &time_out);
118 if (time_out)
119 {
120 // cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " -
121 // measurement Timed-Out" << endl;
122 std::this_thread::sleep_for(10ms);
123 }
124 else
125 have_reading = true;
126 }
127
128 // cout << "[CRaePID] " << com_port << " @ " <<com_bauds << " - measurement
129 // -> " << power_reading << endl;
130
131 // Convert the text to a number (ppm)
132 const float readnum = atof(power_reading.c_str());
133 const float val_ppm = readnum / 1000;
134
135 // Fill the observation
136 mrpt::obs::CObservationGasSensors::TObservationENose obs;
137 obs.readingsVoltage.push_back(val_ppm);
138 obs.sensorTypes.push_back(0x0001);
139
140 mrpt::obs::CObservationGasSensors obsG;
141 obsG.sensorLabel = this->getSensorLabel();
142 obsG.m_readings.push_back(obs);
143 obsG.timestamp = mrpt::Clock::now();
144
145 appendObservation(mrpt::obs::CObservationGasSensors::Create(obsG));
146 }
147
148 std::string CRaePID::getFirmware()
149 {
150 // Send the command
151 cout << "Firmware version: " << endl;
152 COM.purgeBuffers();
153 size_t B_written = COM.Write("F", 1);
154 if (!B_written) return std::string("COMMS.ERROR");
155
156 // Read the returned text
157 bool time_out = false;
158 std::string s_read = COM.ReadString(2000, &time_out);
159 if (time_out) s_read = "Time_out";
160 return s_read;
161 }
162
163 std::string CRaePID::getModel()
164 {
165 // Send the command
166 COM.purgeBuffers();
167 COM.Write("M", 1);
168
169 // Read the returned text
170 return COM.ReadString();
171 }
172
173 std::string CRaePID::getSerialNumber()
174 {
175 // Send the command
176 COM.purgeBuffers();
177 COM.Write("S", 1);
178
179 // Read the returned text
180 return COM.ReadString();
181 }
182
183 std::string CRaePID::getName()
184 {
185 // Send the command
186 COM.purgeBuffers();
187 COM.Write("N", 1);
188
189 // Read the returned text
190 return COM.ReadString();
191 }
192
193 bool CRaePID::switchPower()
194 {
195 // Send the command
196 COM.purgeBuffers();
197 COM.Write("P", 1);
198
199 // Read the returned text
200 std::string reading;
201 reading = COM.ReadString();
202
203 if (strcmp(reading.c_str(), "Sleep...") == 0)
204 return true;
205 else
206 return false;
207 }
208
209 mrpt::obs::CObservationGasSensors CRaePID::getFullInfo()
210 {
211 // Send the command
212 COM.purgeBuffers();
213 COM.Write("C", 1);
214
215 // Read the returned text
216 std::string reading;
217 reading = COM.ReadString();
218
219 // Iterate over each information component (tokenize first)
220 // construct a stream from the string (as seen in
221 // http://stackoverflow.com/a/53921)
222 std::stringstream readings_str(reading);
223
224 // use stream iterators to copy the stream to the vector as whitespace
225 // separated strings
226 std::istream_iterator<std::string> it(readings_str);
227 std::istream_iterator<std::string> endit;
228 std::vector<std::string> measurements_text(it, endit);
229
230 // Convert the text to a number (ppm)
231 mrpt::obs::CObservationGasSensors::TObservationENose obs;
232 mrpt::obs::CObservationGasSensors obsG;
233
234 for (auto& k : measurements_text)
235 {
236 const float readnum = atof(k.c_str());
237 const float val_ppm = readnum / 1000.f;
238
239 // Fill the observation
240 obs.readingsVoltage.push_back(val_ppm);
241 obsG.m_readings.push_back(obs);
242 }
243
244 obsG.sensorLabel = this->getSensorLabel();
245 obsG.timestamp = mrpt::Clock::now();
246
247 return obsG;
248 }
249
250 bool CRaePID::errorStatus(std::string& errorString)
251 {
252 // Send the command
253 COM.purgeBuffers();
254 COM.Write("E", 1);
255
256 // Read the returned text
257 std::string reading;
258 reading = COM.ReadString();
259
260 // Tokenize to separate the two components:
261 // construct a stream from the string (as seen in
262 // http://stackoverflow.com/a/53921)
263 std::stringstream readings_str(reading);
264
265 // use stream iterators to copy the stream to the vector as whitespace
266 // separated strings
267 std::istream_iterator<std::string> it(readings_str);
268 std::istream_iterator<std::string> endit;
269 std::vector<std::string> errors_text(it, endit);
270
271 // Take the first part and check the possible error condition
272 if ((strcmp(errors_text[0].c_str(), "0") == 0) &&
273 (strcmp(errors_text[1].c_str(), "0") == 0)) // no error
274 {
275 return false;
276 }
277 else
278 {
279 // By the moment, return the raw error; note that if necessary a
280 // detailed description of the error can be obtained analyzing the two
281 // error strings separately
282 errorString = reading;
283 return true;
284 }
285 }
286
287 void CRaePID::getLimits(float& min, float& max)
288 {
289 // Send the command
290 COM.purgeBuffers();
291 COM.Write("L", 1);
292
293 // Read the returned text
294 std::string reading;
295 reading = COM.ReadString();
296
297 // Tokenize to separate the two components:
298 // construct a stream from the string (as seen in
299 // http://stackoverflow.com/a/53921)
300 std::stringstream readings_str(reading);
301
302 // use stream iterators to copy the stream to the vector as whitespace
303 // separated strings
304 std::istream_iterator<std::string> it(readings_str);
305 std::istream_iterator<std::string> endit;
306 std::vector<std::string> readings_text(it, endit);
307
308 // read min and max
309 max = atof(readings_text[0].c_str());
310 min = atof(readings_text[1].c_str());
311 }