Debian Codegraph @

libs/hwdrivers/src/CGyroKVHDSP3000.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/CGyroKVHDSP3000.h>
13 #include <mrpt/obs/CObservationIMU.h>
14
15 #include <chrono>
16 #include <iostream>
17 #include <thread>
18
19 IMPLEMENTS_GENERIC_SENSOR(CGyroKVHDSP3000, mrpt::hwdrivers)
20
21 using namespace mrpt::comms;
22 using namespace mrpt::obs;
23 using namespace mrpt::hwdrivers;
24 using namespace std;
25
26 /*-------------------------------------------------------------
27 CGyroKVHDSP3000
28 -------------------------------------------------------------*/
29 CGyroKVHDSP3000::CGyroKVHDSP3000() : m_com_port(), m_sensorPose()
30
31 {
32 m_state = ssInitializing;
33 m_sensorLabel = "KVH_DSP3000";
34 }
35
36 CGyroKVHDSP3000::~CGyroKVHDSP3000() { m_serialPort->close(); }
37
38 /*-------------------------------------------------------------
39 doProcess
40 -------------------------------------------------------------*/
41 void CGyroKVHDSP3000::doProcess()
42 {
43 if (m_state == ssError)
44 {
45 std::this_thread::sleep_for(200ms);
46 initialize();
47 }
48
49 if (m_state == ssError) return;
50
51 string msg;
52 CObservationIMU::Ptr observationGyro = std::make_shared<CObservationIMU>();
53 observationGyro->timestamp = mrpt::Clock::now();
54
55 msg = m_serialPort->ReadString(-1, nullptr, "\n");
56
57 observationGyro->sensorPose = m_sensorPose;
58 observationGyro->sensorLabel = m_sensorLabel;
59 string delimiter(" ");
60 vector<string> words;
61 mrpt::system::tokenize(msg, delimiter, words);
62 if (words.size() < 2) return;
63 if (words[1].c_str()[0] == '0') return;
64 double mesure = atof(words[0].c_str());
65 switch (m_mode)
66 {
67 case RATE:
68 observationGyro->rawMeasurements[IMU_YAW_VEL] = DEG2RAD(mesure);
69 observationGyro->dataIsPresent[IMU_YAW_VEL] = true;
70 break;
71 case INTEGRATED_ANGLE:
72 case INCREMENTAL_ANGLE:
73 observationGyro->rawMeasurements[IMU_YAW] = DEG2RAD(mesure);
74 observationGyro->dataIsPresent[IMU_YAW] = true;
75 break;
76 }
77
78 if (!m_firstInteration)
79 {
80 appendObservation(observationGyro);
81 }
82 else
83 m_firstInteration = false;
84 }
85
86 /*-------------------------------------------------------------
87 initialize
88 -------------------------------------------------------------*/
89 void CGyroKVHDSP3000::initialize()
90 {
91 m_process_rate = 100;
92
93 /*
94 Open modem device for reading and writing and not as controlling tty
95 because we don't want to get killed if linenoise sends CTRL-C.
96 */
97 m_serialPort = std::make_unique<CSerialPort>(m_com_port);
98 if (!(m_serialPort->isOpen())) THROW_EXCEPTION("can't open serial port");
99 cout << "m_COMbaud " << m_COMbauds << endl;
100 m_serialPort->setConfig(m_COMbauds);
101
102 changeMode(m_mode);
103 resetIncrementalAngle();
104 m_state = ssWorking;
105 }
106
107 /*-------------------------------------------------------------
108 loadConfig_sensorSpecific
109 -------------------------------------------------------------*/
110 void CGyroKVHDSP3000::loadConfig_sensorSpecific(
111 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
112 {
113 m_sensorPose.setFromValues(
114 configSource.read_float(iniSection, "pose_x", 0, false),
115 configSource.read_float(iniSection, "pose_y", 0, false),
116 configSource.read_float(iniSection, "pose_z", 0, false),
117 DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
118 DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
119 DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
120 string operatingMode = configSource.read_string(iniSection, "operatingMode", "rate", false);
121 cout << "Operating mode : " << operatingMode << endl;
122 if (operatingMode == "incremental")
123 {
124 m_mode = INCREMENTAL_ANGLE;
125 cout << "Incremental mode" << endl;
126 }
127 else if (operatingMode == "integral")
128 {
129 m_mode = INTEGRATED_ANGLE;
130 cout << "Integrated mode" << endl;
131 }
132 else
133 {
134 m_mode = RATE;
135 cout << "Rate mode" << endl;
136 }
137 m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
138 }
139
140 void CGyroKVHDSP3000::changeMode(GYRO_MODE _newMode)
141 {
142 m_mode = _newMode;
143 char commande[3];
144 switch (m_mode)
145 {
146 case RATE:
147 commande[0] = 'R';
148 break;
149 case INTEGRATED_ANGLE:
150 commande[0] = 'P';
151 break;
152 case INCREMENTAL_ANGLE:
153 commande[0] = 'A'; // incremental.
154 break;
155 }
156 commande[1] = 0x0A;
157 commande[2] = 0;
158 // we send the command four times to be sure that the command will be
159 // interpreted by the sensor.
160 if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
161 {
162 THROW_EXCEPTION("can't write on serial port");
163 }
164 }
165
166 void CGyroKVHDSP3000::resetIncrementalAngle()
167 {
168 if (m_mode != RATE)
169 {
170 char commande[3];
171 commande[0] = 'Z';
172 commande[1] = '\n';
173 commande[2] = 0;
174 if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
175 {
176 THROW_EXCEPTION("can't write on serial port");
177 }
178 }
179 }