Debian Codegraph @

libs/hwdrivers/src/CGillAnemometer.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/CGillAnemometer.h>
13 #include <mrpt/system/datetime.h>
14
15 #include <chrono>
16 #include <iostream>
17 #include <iterator>
18 #include <sstream>
19 #include <thread>
20
21 using namespace std;
22 using namespace mrpt::hwdrivers;
23
24 IMPLEMENTS_GENERIC_SENSOR(CGillAnemometer, mrpt::hwdrivers)
25
26 /* -----------------------------------------------------
27 Constructor
28 ----------------------------------------------------- */
29 CGillAnemometer::CGillAnemometer() { m_sensorLabel = "WINDSONIC"; }
30 /* -----------------------------------------------------
31 loadConfig_sensorSpecific
32 ----------------------------------------------------- */
33 void CGillAnemometer::loadConfig_sensorSpecific(
34 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
35 {
36 #ifdef _WIN32
37 com_port = configSource.read_string(iniSection, "COM_port_WIN", "COM1", true);
38 #else
39 com_port = configSource.read_string(iniSection, "COM_port_LIN", "/dev/tty0", true);
40 #endif
41
42 com_bauds = configSource.read_int(iniSection, "COM_baudRate", 9600, false);
43
44 pose_x = configSource.read_float(iniSection, "pose_x", 0, true);
45 pose_y = configSource.read_float(iniSection, "pose_y", 0, true);
46 pose_z = configSource.read_float(iniSection, "pose_z", 0, true);
47 pose_roll = configSource.read_float(iniSection, "pose_roll", 0, true);
48 pose_pitch = configSource.read_float(iniSection, "pose_pitch", 0, true);
49 pose_yaw = configSource.read_float(iniSection, "pose_yaw", 0, true);
50 }
51
52 /* -----------------------------------------------------
53 tryToOpenTheCOM
54 ----------------------------------------------------- */
55 bool CGillAnemometer::tryToOpenTheCOM()
56 {
57 if (COM.isOpen()) return true; // Already open
58
59 if (m_verbose) cout << "[CGillAnemometer] Opening " << com_port << " @ " << com_bauds << endl;
60
61 try
62 {
63 COM.open(com_port);
64 COM.setConfig(com_bauds, 0, 8, 1);
65 COM.setTimeouts(50, 1, 100, 1, 20);
66 COM.purgeBuffers();
67
68 return true; // All OK!
69 }
70 catch (const std::exception& e)
71 {
72 std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
73 "configuring the serial port:"
74 << std::endl
75 << e.what();
76 COM.close();
77 return false;
78 }
79 catch (...)
80 {
81 std::cerr << "[CGillAnemometer::tryToOpenTheCOM] Error opening or "
82 "configuring the serial port."
83 << std::endl;
84 COM.close();
85 return false;
86 }
87 }
88
89 /* -----------------------------------------------------
90 doProcess
91 ----------------------------------------------------- */
92 void CGillAnemometer::doProcess()
93 {
94 // Is the COM open?
95 if (!tryToOpenTheCOM())
96 {
97 m_state = ssError;
98 printf("ERROR: No observation received from the Anemometer!\n");
99 THROW_EXCEPTION("Cannot open the serial port");
100 }
101
102 mrpt::obs::CObservationWindSensor::Ptr obsPtr = mrpt::obs::CObservationWindSensor::Create();
103 bool have_reading = false;
104 std::string wind_reading;
105 bool time_out = false;
106
107 try
108 {
109 while (!have_reading)
110 {
111 // Read info into string: (default is Polar continuous)
112 // format = <STX>Q, dir, speed, units, status, <ETX>
113 // Q -> Sensor identifier
114 // dir -> 3 digits (0-359)
115 // speed -> %3.2f
116 // units -> M (m/s), K (km/h)
117 // status -> 00 = ok, else is an Error Code
118
119 wind_reading = COM.ReadString(500, &time_out);
120 if (time_out)
121 {
122 cout << "[CGillAnemometer] " << com_port << " @ " << com_bauds << " - measurement Timed-Out"
123 << endl;
124 std::this_thread::sleep_for(10ms);
125 }
126 else
127 have_reading = true;
128 }
129
130 // parse format = <STX>Q, dir, speed, units, status, <ETX>
131 std::deque<std::string> list;
132 mrpt::system::tokenize(wind_reading, ",", list);
133 if (list.size() == 6)
134 {
135 // Status
136 int status = atoi(list.at(4).c_str());
137 if (status == 0)
138 {
139 // Units
140 std::string s_units = list.at(3);
141 // Module
142 std::string s_speed = list.at(2);
143 if (s_units == "M")
144 obsPtr->speed = atof(s_speed.c_str());
145 else if (s_units == "K")
146 obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
147 else
148 {
149 printf(
150 "ERROR: WindSonic measurement units not supported: "
151 "%s\n",
152 s_units.c_str());
153 obsPtr->speed = 0.0;
154 }
155 // angle
156 std::string s_direction = list.at(1);
157 obsPtr->direction = atof(s_direction.c_str());
158
159 // Prepare observation
160 obsPtr->sensorLabel = m_sensorLabel;
161 obsPtr->timestamp = mrpt::Clock::now();
162 obsPtr->sensorPoseOnRobot =
163 mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
164 appendObservation(obsPtr);
165 }
166 else
167 printf("ERROR: WindSonic error code %u\n", status);
168 }
169 else if (list.size() == 5) // when there is no wind, the sensor does
170 // not provide a wind direction
171 {
172 // Status
173 int status = atoi(list.at(3).c_str());
174 if (status == 0)
175 {
176 // Units
177 std::string s_units = list.at(2);
178 // module
179 std::string s_speed = list.at(1);
180 if (s_units == "M")
181 obsPtr->speed = atof(s_speed.c_str());
182 else if (s_units == "K")
183 obsPtr->speed = atof(s_speed.c_str()) * 1000 / 3600;
184 else
185 {
186 printf(
187 "ERROR: WindSonic measurement units not supported: "
188 "%s\n",
189 s_units.c_str());
190 obsPtr->speed = 0.0;
191 }
192 // Angle
193 obsPtr->direction = 0.0;
194
195 // Prepare observation
196 obsPtr->sensorLabel = m_sensorLabel;
197 obsPtr->timestamp = mrpt::Clock::now();
198 obsPtr->sensorPoseOnRobot =
199 mrpt::poses::CPose3D(pose_x, pose_y, pose_z, pose_yaw, pose_pitch, pose_roll);
200 appendObservation(obsPtr);
201 }
202 else
203 printf("ERROR: WindSonic error code %u\n", status);
204 }
205 else
206 {
207 printf(
208 "ERROR: Windsonic reading incorrect format: %s [%u]\n", wind_reading.c_str(),
209 static_cast<unsigned int>(list.size()));
210 }
211 }
212 catch (const std::exception& e)
213 {
214 std::cerr << "[CGillAnemometer::doProcess] Error:" << std::endl << e.what();
215 }
216 catch (...)
217 {
218 std::cerr << "[CGillAnemometer::doProcess] Unknown Error" << std::endl;
219 }
220 }