Debian Codegraph @

libs/hwdrivers/src/CTaoboticsIMU.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/core/bit_cast.h>
13 #include <mrpt/core/reverse_bytes.h>
14 #include <mrpt/hwdrivers/CTaoboticsIMU.h>
15 #include <mrpt/obs/CObservationIMU.h>
16
17 #include <chrono>
18 #include <iostream>
19 #include <thread>
20
21 IMPLEMENTS_GENERIC_SENSOR(CTaoboticsIMU, mrpt::hwdrivers)
22
23 using namespace mrpt::comms;
24 using namespace mrpt::obs;
25 using namespace mrpt::hwdrivers;
26
27 CTaoboticsIMU::CTaoboticsIMU()
28 {
29 m_state = ssInitializing;
30 m_sensorLabel = "IMU";
31 }
32
33 CTaoboticsIMU::~CTaoboticsIMU()
34 {
35 if (m_serialPort) m_serialPort->close();
36 }
37
38 void CTaoboticsIMU::setSerialPort(const std::string& serialPort)
39 {
40 ASSERTMSG_(!m_serialPort, "setSerialPort() can be called only before initialize()");
41
42 m_com_port = serialPort;
43 }
44
45 void CTaoboticsIMU::setSerialBaudRate(int rate)
46 {
47 ASSERTMSG_(!m_serialPort, "setSerialBaudRate() can be called only before initialize()");
48
49 m_baudRate = rate;
50 }
51
52 void CTaoboticsIMU::doProcess()
53 {
54 using namespace std::chrono_literals;
55
56 ASSERTMSG_(m_activeParser, "initialize() must be called first");
57 ASSERT_(m_serialPort);
58
59 if (m_state == ssError)
60 {
61 std::this_thread::sleep_for(200ms);
62 initialize();
63 }
64
65 if (m_state == ssError) return;
66
67 // try to read and parse a frame from the serial port:
68 std::vector<uint8_t> buf(m_rx_buffer.available());
69
70 try
71 {
72 const auto nRead = m_serialPort->ReadBufferImmediate(buf.data(), buf.size());
73 m_rx_buffer.push_many(buf.data(), nRead);
74 }
75 catch (const std::exception& e)
76 {
77 std::cerr << "[CTaobotics] Error readingopen serial port " << m_com_port << std::endl;
78 m_state = ssError;
79 }
80
81 // Parse:
82 ASSERT_(m_activeParser);
83 const auto allObs = m_activeParser(this, m_rx_buffer);
84
85 // and send observations out, if any:
86 for (const auto& obs : allObs) this->appendObservation(obs);
87 }
88
89 void CTaoboticsIMU::initialize()
90 {
91 if (m_sensorModel == "hfi-b6")
92 m_activeParser = &CTaoboticsIMU::parser_hfi_b6;
93 else if (m_sensorModel == "hfi-a9")
94 m_activeParser = &CTaoboticsIMU::parser_hfi_a9;
95 else
96 {
97 THROW_EXCEPTION_FMT("Unknown sensor model: '%s'", m_sensorModel.c_str());
98 }
99
100 if (m_verbose)
101 std::cout << "[CTaoboticsIMU] Opening port: " << m_com_port << " at " << m_baudRate
102 << " bauds.\n";
103
104 try
105 {
106 m_serialPort = std::make_unique<CSerialPort>(m_com_port);
107 ASSERT_(m_serialPort && m_serialPort->isOpen());
108
109 m_serialPort->setConfig(m_baudRate);
110 m_serialPort->setTimeouts(1, 1, 1, 1, 1);
111
112 m_serialPort->purgeBuffers();
113 m_rx_buffer.clear();
114
115 m_state = ssWorking;
116 }
117 catch (const std::exception& e)
118 {
119 std::cerr << "[CTaobotics] Can't open serial port " << m_com_port << std::endl;
120 m_state = ssError;
121 }
122 }
123
124 void CTaoboticsIMU::loadConfig_sensorSpecific(
125 const mrpt::config::CConfigFileBase& c, const std::string& s)
126 {
127 m_sensorPose.setFromValues(
128 c.read_float(s, "pose_x", 0, false), c.read_float(s, "pose_y", 0, false),
129 c.read_float(s, "pose_z", 0, false), DEG2RAD(c.read_float(s, "pose_yaw", 0, false)),
130 DEG2RAD(c.read_float(s, "pose_pitch", 0, false)),
131 DEG2RAD(c.read_float(s, "pose_roll", 0, false)));
132
133 m_com_port = c.read_string(s, "serialPort", m_com_port);
134 m_sensorModel = c.read_string(s, "sensorModel", m_sensorModel);
135 }
136
137 std::vector<mrpt::obs::CObservation::Ptr> CTaoboticsIMU::parser_hfi_b6(
138 mrpt::containers::circular_buffer<uint8_t>& buf) const
139 {
140 std::vector<mrpt::obs::CObservation::Ptr> out;
141
142 // Parse:
143 // Header must be: "0x55 0x5*".
144 // But we must wait for the first "0x55 0x51" to ensure synchronization
145 // of all IMU data fields in one observation:
146 while (buf.size() >= 11)
147 {
148 if (buf.peek(0) == 0x55 && buf.peek(1) == 0x51)
149 {
150 break;
151 }
152 else
153 {
154 // discard byte:
155 buf.pop();
156 }
157 }
158
159 // Actual processing, of one or more observations:
160 mrpt::obs::CObservationIMU::Ptr obs;
161
162 while (buf.size() >= 11)
163 {
164 if (buf.peek(0) == 0x55 && (buf.peek(1) & 0x50) == 0x50)
165 {
166 std::array<uint8_t, 11> frame;
167 buf.pop_many(frame.data(), frame.size());
168
169 // Parse this frame. Each frame seems to be fixed size = 11
170 // bytes.
171 //
172 // Frame field : 0x55 | TYPE | D0 | D1 | D2 | D3 |CHKSUM
173 // Byte index : 0 | 1 | 2 3 | 4 5 | 6 7 | 8 9 | 10
174 //
175 // Type:
176 // 0x51: acceleration, 0x52: angular velocity, 0x53: euler
177 // angles
178 //
179 // TODO: Checksum
180
181 uint16_t data[4];
182 for (int i = 0; i < 4; i++)
183 {
184 data[i] = frame[2 + 2 * i] | (frame[2 + 2 * i + 1] << 8);
185 mrpt::toNativeEndianness(data[i]);
186 }
187
188 switch (frame[1])
189 {
190 case 0x51:
191 {
192 if (!obs) obs = std::make_shared<CObservationIMU>();
193 const double accx = data[0] / 32768.0 * 16 * -9.8;
194 const double accy = data[1] / 32768.0 * 16 * -9.8;
195 const double accz = data[2] / 32768.0 * 16 * -9.8;
196 printf("acc: %f %f %f\n", accx, accy, accz);
197 }
198 break;
199 case 0x52:
200 {
201 if (!obs) obs = std::make_shared<CObservationIMU>();
202 //
203 }
204 break;
205 case 0x53:
206 {
207 // and send out one obs:
208 if (!obs) obs = std::make_shared<CObservationIMU>();
209
210 // get euler angle:
211
212 // send out:
213 obs->timestamp = mrpt::Clock::now();
214
215 obs->sensorPose = m_sensorPose;
216 obs->sensorLabel = m_sensorLabel;
217
218 out.push_back(obs);
219 obs.reset();
220 }
221 break;
222 default:
223 fprintf(
224 stderr,
225 "[CTaoboticsIMU] Received frame with unknown data type "
226 "id=0x%02X\n",
227 frame[1]);
228 break;
229 }
230 }
231 else
232 {
233 // discard byte:
234 buf.pop();
235 }
236 }
237
238 return out;
239 }
240
241 std::vector<mrpt::obs::CObservation::Ptr> CTaoboticsIMU::parser_hfi_a9(
242 mrpt::containers::circular_buffer<uint8_t>& buf) const
243 {
244 using namespace mrpt::obs;
245
246 std::vector<mrpt::obs::CObservation::Ptr> out;
247
248 // Parse. See .h for data frame format
249 while (buf.size() >= 49 + 25)
250 {
251 if (buf.peek(0) == 0xAA && buf.peek(1) == 0x55 && buf.peek(2) == 0x2c)
252 {
253 break;
254 }
255 else
256 {
257 // discard byte:
258 buf.pop();
259 }
260 }
261
262 // Actual processing, of one or more observations:
263 mrpt::obs::CObservationIMU::Ptr obs;
264
265 while (buf.size() >= 49 + 25)
266 {
267 if (buf.peek(0) == 0xAA && buf.peek(1) == 0x55 && buf.peek(2) == 0x2c)
268 {
269 std::array<uint8_t, 49> frame;
270 buf.pop_many(frame.data(), frame.size());
271
272 #if 0
273 const uint32_t hwTimestamp = (frame[7 + 3] << 24) | //
274 (frame[7 + 2] << 16) | //
275 (frame[7 + 1] << 8) | //
276 (frame[7 + 0] << 0);
277 #endif
278
279 float data[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
280 for (int i = 0; i < 9; i++)
281 {
282 // Fixed format: little endian for this sensor.
283 const uint32_t d = //
284 (frame[11 + 4 * i + 3] << 24) | //
285 (frame[11 + 4 * i + 2] << 16) | //
286 (frame[11 + 4 * i + 1] << 8) | //
287 (frame[11 + 4 * i + 0] << 0);
288 data[i] = bit_cast<float>(d);
289 }
290
291 if (!obs)
292 {
293 obs = std::make_shared<CObservationIMU>();
294 obs->sensorPose = m_sensorPose;
295 obs->sensorLabel = m_sensorLabel;
296 }
297
298 const float wx = data[0], wy = data[1], wz = data[2];
299 const float ax = data[3], ay = data[4], az = data[5];
300 const float mx = data[6], my = data[7], mz = data[8];
301
302 // send out:
303 obs->timestamp = mrpt::Clock::now();
304
305 obs->set(IMU_X_ACC, -9.8f * ax);
306 obs->set(IMU_Y_ACC, -9.8f * ay);
307 obs->set(IMU_Z_ACC, -9.8f * az);
308
309 obs->set(IMU_WZ, wz);
310 obs->set(IMU_WY, wy);
311 obs->set(IMU_WX, wx);
312
313 obs->set(IMU_MAG_X, mx);
314 obs->set(IMU_MAG_Y, my);
315 obs->set(IMU_MAG_Z, mz);
316
317 // do not send out the obs yet, we want to also add the ypr angles
318 // in the next frame:
319 }
320 else
321 {
322 // discard byte:
323 buf.pop();
324 }
325
326 if (buf.peek(0) == 0xAA && buf.peek(1) == 0x55 && buf.peek(2) == 0x14)
327 {
328 std::array<uint8_t, 25> frame;
329 buf.pop_many(frame.data(), frame.size());
330
331 #if 0
332 const uint32_t hwTimestamp = (frame[7 + 3] << 24) | //
333 (frame[7 + 2] << 16) | //
334 (frame[7 + 1] << 8) | //
335 (frame[7 + 0] << 0);
336 #endif
337
338 float data[3] = {0, 0, 0};
339 for (int i = 0; i < 3; i++)
340 {
341 // Fixed format: little endian for this sensor.
342 const uint32_t d = //
343 (frame[11 + 4 * i + 3] << 24) | //
344 (frame[11 + 4 * i + 2] << 16) | //
345 (frame[11 + 4 * i + 1] << 8) | //
346 (frame[11 + 4 * i + 0] << 0);
347 data[i] = bit_cast<float>(d);
348 }
349
350 if (!obs)
351 {
352 obs = std::make_shared<CObservationIMU>();
353 obs->sensorPose = m_sensorPose;
354 obs->sensorLabel = m_sensorLabel;
355 }
356
357 const float roll = mrpt::DEG2RAD(data[0]), pitch = mrpt::DEG2RAD(-data[1]),
358 yaw = mrpt::DEG2RAD(-data[2]);
359
360 // send out:
361 obs->timestamp = mrpt::Clock::now();
362
363 mrpt::math::CQuaternionDouble q;
364 mrpt::poses::CPose3D::FromYawPitchRoll(yaw, pitch, roll).getAsQuaternion(q);
365
366 obs->set(IMU_ORI_QUAT_W, q.r());
367 obs->set(IMU_ORI_QUAT_X, q.x());
368 obs->set(IMU_ORI_QUAT_Y, q.y());
369 obs->set(IMU_ORI_QUAT_Z, q.z());
370
371 out.push_back(obs);
372 obs.reset();
373 }
374 else
375 {
376 // discard byte:
377 buf.pop();
378 }
379 }
380
381 return out;
382 }