Debian Codegraph @

libs/hwdrivers/src/CEnoseModular.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/CEnoseModular.h>
13 #include <mrpt/serialization/CArchive.h>
14 #include <mrpt/serialization/CMessage.h>
15 #include <mrpt/system/os.h>
16
17 #include <iostream>
18 #include <memory>
19 #include <thread>
20
21 using namespace mrpt::math;
22 using namespace mrpt::obs;
23 using namespace mrpt::poses;
24 using namespace mrpt::hwdrivers;
25 using namespace mrpt::io;
26 using namespace std;
27
28 IMPLEMENTS_GENERIC_SENSOR(CEnoseModular, mrpt::hwdrivers)
29
30 /*-------------------------------------------------------------
31 CEnoseModular
32 -------------------------------------------------------------*/
33 CEnoseModular::CEnoseModular() : m_usbSerialNumber("ENOSE002"), m_COM_port()
34 {
35 m_sensorLabel = "EnoseModular";
36 first_reading = true;
37 }
38
39 /*-------------------------------------------------------------
40 loadConfig_sensorSpecific
41 -------------------------------------------------------------*/
42 void CEnoseModular::loadConfig_sensorSpecific(
43 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
44 {
45 MRPT_START
46
47 m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname", "", false);
48
49 #ifdef _WIN32
50 m_COM_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1");
51 #else
52 m_COM_port = configSource.read_string(iniSection, "COM_port_LIN", m_COM_port);
53 #endif
54 m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate", m_COM_baud);
55
56 MRPT_END
57 }
58
59 /*-------------------------------------------------------------
60 checkConnectionAndConnect
61 -------------------------------------------------------------*/
62 CStream* CEnoseModular::checkConnectionAndConnect()
63 {
64 // Make sure one of the two possible pipes is open:
65 if (!m_stream_FTDI && !m_stream_SERIAL)
66 {
67 if (!m_COM_port.empty())
68 m_stream_SERIAL = std::make_unique<mrpt::comms::CSerialPort>();
69 else
70 m_stream_FTDI = std::make_unique<mrpt::comms::CInterfaceFTDI>();
71 }
72
73 if (m_stream_FTDI)
74 { // FTDI pipe ==================
75 if (m_stream_FTDI->isOpen()) return m_stream_FTDI.get();
76 try
77 {
78 m_stream_FTDI->OpenBySerialNumber(m_usbSerialNumber);
79 std::this_thread::sleep_for(10ms);
80 m_stream_FTDI->Purge();
81 std::this_thread::sleep_for(10ms);
82 m_stream_FTDI->SetLatencyTimer(1);
83 m_stream_FTDI->SetTimeouts(10, 100);
84 return m_stream_FTDI.get();
85 }
86 catch (...)
87 { // Error opening device:
88 m_stream_FTDI->Close();
89 return nullptr;
90 }
91 }
92 else
93 { // Serial pipe ==================
94 ASSERT_(m_stream_SERIAL);
95 if (m_stream_SERIAL->isOpen()) return m_stream_SERIAL.get();
96 try
97 {
98 m_stream_SERIAL->open(m_COM_port);
99 m_stream_SERIAL->setConfig(m_COM_baud);
100 // m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
101 m_stream_SERIAL->setTimeouts(50, 1, 100, 1, 20);
102 std::this_thread::sleep_for(10ms);
103 m_stream_SERIAL->purgeBuffers();
104 std::this_thread::sleep_for(10ms);
105 return m_stream_SERIAL.get();
106 }
107 catch (...)
108 { // Error opening device:
109 m_stream_SERIAL->close();
110 return nullptr;
111 }
112 }
113 }
114
115 /*-------------------------------------------------------------
116 getObservation
117 -------------------------------------------------------------*/
118 bool CEnoseModular::getObservation(mrpt::obs::CObservationGasSensors& obs)
119 {
120 try
121 {
122 // Connected?
123 CStream* comms = checkConnectionAndConnect();
124
125 if (!comms)
126 {
127 cout << "ERORR: Problem connecting to Device." << endl;
128 return false;
129 }
130
131 CObservationGasSensors::TObservationENose newRead;
132 obs.m_readings.clear();
133
134 //---------------------------- Enose Modular FRAME
135 //--------------------------------------------------
136 // Wait for e-nose frame: <0x69><0x91><length><body><0x96> "Bytes"
137 // Where <body> = <temp>[<SensorID_H><SensorID_L><Sensor_Value>] x
138 // N_senosrs
139 // Modular-nose provides a 4B+body frame length
140
141 mrpt::serialization::CMessage msg;
142 bool time_out = false;
143 mrpt::system::TTimeStamp t1 = mrpt::Clock::now();
144 double time_out_val = 1; // seconds
145
146 auto arch = mrpt::serialization::archiveFrom(*comms);
147
148 while (!arch.receiveMessage(msg) && !time_out)
149 {
150 if (mrpt::system::timeDifference(t1, mrpt::Clock::now()) > time_out_val) time_out = true;
151 }
152
153 if (time_out)
154 {
155 cout << "[CEnoseModular - getObservation] measurement Timed-Out" << endl;
156 return false;
157 }
158
159 if (msg.content.size() > 0)
160 {
161 // Each sensor reading is composed of 3 Bytes
162 // [<SensorID_H><SensorID_L><Sensor_Value>]
163 ASSERT_((msg.content.size() - 1) % 3 == 0);
164 size_t numSensors = (msg.content.size() - 1) / 3;
165
166 // Prepare the Enose observation
167 newRead.sensorTypes.clear();
168 newRead.readingsVoltage.clear();
169 newRead.hasTemperature = true;
170 newRead.isActive = true;
171
172 // Do we have the sensor position?
173 if (enose_poses_x.size() != 0)
174 {
175 newRead.eNosePoseOnTheRobot = TPose3D(
176 enose_poses_x[0], enose_poses_y[0], enose_poses_z[0], enose_poses_yaw[0],
177 enose_poses_pitch[0], enose_poses_roll[0]);
178 }
179 else
180 newRead.eNosePoseOnTheRobot = TPose3D(0, 0, 0, 0, 0, 0);
181
182 // Get Temperature (degrees C)
183 newRead.temperature = msg.content[0] * 1.65214 - 277.74648;
184
185 // process all sensors
186 for (size_t idx = 0; idx < numSensors; idx++)
187 {
188 // Copy ID (2 BYTES) To integer
189 int sensorType_temp = 0;
190 // memcpy( &sensorType, &msg.content[idx*3+1],
191 // 2*sizeof(msg.content[0]) );
192 memcpy(&sensorType_temp, &msg.content[idx * 3 + 1], sizeof(msg.content[0]));
193 int sensorType = sensorType_temp << (8);
194 memcpy(&sensorType, &msg.content[idx * 3 + 2], sizeof(msg.content[0]));
195
196 // Add sensor Type (ID)
197 newRead.sensorTypes.push_back(sensorType);
198
199 // Transform from ADC value[8bits] to [0-0.6] volt range:
200 newRead.readingsVoltage.push_back((msg.content[idx * 3 + 3] * 0.6f) / 255.0f);
201 }
202
203 // Purge buffers
204 purgeBuffers();
205
206 // Add data to observation:
207 obs.m_readings.push_back(newRead);
208 obs.sensorLabel = m_sensorLabel;
209 obs.timestamp = mrpt::Clock::now();
210 return !obs.m_readings.empty(); // Done OK!
211 }
212 else
213 {
214 cout << "Message was empty" << endl;
215 return false;
216 }
217 }
218 catch (exception& e)
219 {
220 cerr << "[CEnoseModular::getObservation] Returning false due to "
221 "exception: "
222 << endl;
223 cerr << e.what() << endl;
224 return false;
225 }
226 catch (...)
227 {
228 return false;
229 }
230 }
231
232 /*-------------------------------------------------------------
233 doProcess
234 -------------------------------------------------------------*/
235 /** This method should be called periodically (at least at 1Hz to capture ALL
236 * the real-time data)
237 * It is thread safe, i.e. you can call this from one thread, then to other
238 * methods from other threads.
239 */
240 void CEnoseModular::doProcess()
241 {
242 CObservationGasSensors::Ptr obs = std::make_shared<CObservationGasSensors>();
243
244 if (getObservation(*obs))
245 {
246 m_state = ssWorking;
247 appendObservation(obs);
248 }
249 else
250 {
251 m_state = ssError;
252 cout << "No observation received from the USB board!" << endl;
253 // THROW_EXCEPTION("No observation received from the USB board!");
254 }
255 }
256
257 /*-------------------------------------------------------------
258 purgeBuffers
259 -------------------------------------------------------------*/
260 void CEnoseModular::purgeBuffers()
261 {
262 if (!checkConnectionAndConnect()) return;
263
264 if (m_stream_FTDI)
265 { // FTDI pipe
266 m_stream_FTDI->Purge();
267 }
268 else
269 { // Serial pipe
270 m_stream_SERIAL->purgeBuffers();
271 }
272 }