Main MRPT website > C++ reference for MRPT 1.9.9
CGyroKVHDSP3000.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
14 
15 #include <thread>
16 #include <iostream>
17 
19 
20 using namespace mrpt::comms;
21 using namespace mrpt::obs;
22 using namespace mrpt::hwdrivers;
23 using namespace std;
24 
25 /*-------------------------------------------------------------
26  CGyroKVHDSP3000
27 -------------------------------------------------------------*/
28 CGyroKVHDSP3000::CGyroKVHDSP3000()
29  : m_COMbauds(38400),
30  m_com_port(),
31  m_sensorPose(),
32  m_mode(RATE),
33  m_firstInteration(true)
34 {
36  m_sensorLabel = "KVH_DSP3000";
37  m_serialPort = nullptr;
38 }
39 
40 /*-------------------------------------------------------------
41  ~CGyroKVHDSP3000
42 -------------------------------------------------------------*/
44 {
46  delete m_serialPort;
47 }
48 
49 /*-------------------------------------------------------------
50  doProcess
51 -------------------------------------------------------------*/
53 {
54  if (m_state == ssError)
55  {
56  std::this_thread::sleep_for(200ms);
57  initialize();
58  }
59 
60  if (m_state == ssError) return;
61 
62  string msg;
63  CObservationIMU::Ptr observationGyro =
64  mrpt::make_aligned_shared<CObservationIMU>();
65  observationGyro->timestamp = mrpt::system::now();
66 
67  msg = m_serialPort->ReadString(-1, nullptr, "\n");
68 
69  observationGyro->sensorPose = m_sensorPose;
70  observationGyro->sensorLabel = m_sensorLabel;
71  string delimiter(" ");
72  vector<string> words;
73  mrpt::system::tokenize(msg, delimiter, words);
74  if (words.size() < 2) return;
75  if (words[1].c_str()[0] == '0') return;
76  double mesure = atof(words[0].c_str());
77  switch (m_mode)
78  {
79  case RATE:
80  observationGyro->rawMeasurements[IMU_YAW_VEL] = DEG2RAD(mesure);
81  observationGyro->dataIsPresent[IMU_YAW_VEL] = true;
82  break;
83  case INTEGRATED_ANGLE:
84  case INCREMENTAL_ANGLE:
85  observationGyro->rawMeasurements[IMU_YAW] = DEG2RAD(mesure);
86  observationGyro->dataIsPresent[IMU_YAW] = true;
87  break;
88  }
89 
90  if (!m_firstInteration)
91  {
92  appendObservation(observationGyro);
93  }
94  else
95  m_firstInteration = false;
96 }
97 
98 /*-------------------------------------------------------------
99  initialize
100 -------------------------------------------------------------*/
102 {
103  m_process_rate = 100;
104 
105  /*
106  Open modem device for reading and writing and not as controlling tty
107  because we don't want to get killed if linenoise sends CTRL-C.
108  */
109  if (m_serialPort) delete m_serialPort;
111  if (!(m_serialPort->isOpen())) THROW_EXCEPTION("can't open serial port");
112  cout << "m_COMbaud " << m_COMbauds << endl;
114 
117  m_state = ssWorking;
118 }
119 
120 /*-------------------------------------------------------------
121  loadConfig_sensorSpecific
122 -------------------------------------------------------------*/
124  const mrpt::config::CConfigFileBase& configSource,
125  const std::string& iniSection)
126 {
128  configSource.read_float(iniSection, "pose_x", 0, false),
129  configSource.read_float(iniSection, "pose_y", 0, false),
130  configSource.read_float(iniSection, "pose_z", 0, false),
131  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
132  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
133  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
134  string operatingMode =
135  configSource.read_string(iniSection, "operatingMode", "rate", false);
136  cout << "Operating mode : " << operatingMode << endl;
137  if (operatingMode == "incremental")
138  {
140  cout << "Incremental mode" << endl;
141  }
142  else if (operatingMode == "integral")
143  {
145  cout << "Integrated mode" << endl;
146  }
147  else
148  {
149  m_mode = RATE;
150  cout << "Rate mode" << endl;
151  }
152  m_com_port =
153  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
154 }
155 
157 {
158  m_mode = _newMode;
159  char commande[3];
160  switch (m_mode)
161  {
162  case RATE:
163  commande[0] = 'R';
164  break;
165  case INTEGRATED_ANGLE:
166  commande[0] = 'P';
167  break;
168  case INCREMENTAL_ANGLE:
169  commande[0] = 'A'; // incremental.
170  break;
171  }
172  commande[1] = 0x0A;
173  commande[2] = 0;
174  // we send the command four times to be sure that the command will be
175  // interpreted by the sensor.
176  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
177  {
178  THROW_EXCEPTION("can't write on serial port");
179  }
180 }
181 
183 {
184  if (m_mode != RATE)
185  {
186  char commande[3];
187  commande[0] = 'Z';
188  commande[1] = '\n';
189  commande[2] = 0;
190  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
191  {
192  THROW_EXCEPTION("can't write on serial port");
193  }
194  }
195 }
mrpt::comms::CSerialPort::isOpen
bool isOpen() const
Returns if port has been correctly open.
Definition: CSerialPort.cpp:203
mrpt::hwdrivers::INCREMENTAL_ANGLE
@ INCREMENTAL_ANGLE
Definition: CGyroKVHDSP3000.h:26
mrpt::comms::CSerialPort::close
void close()
Close the port.
Definition: CSerialPort.cpp:638
mrpt::hwdrivers::CGyroKVHDSP3000::doProcess
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
Definition: CGyroKVHDSP3000.cpp:52
mrpt::hwdrivers::CGyroKVHDSP3000::m_sensorPose
mrpt::poses::CPose3D m_sensorPose
Definition: CGyroKVHDSP3000.h:83
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:239
mrpt::hwdrivers::CGyroKVHDSP3000::m_serialPort
mrpt::comms::CSerialPort * m_serialPort
Search the port where the sensor is located and connect to it.
Definition: CGyroKVHDSP3000.h:90
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
mrpt::comms::CSerialPort
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
mrpt::hwdrivers::CGenericSensor::ssWorking
@ ssWorking
Definition: CGenericSensor.h:87
mrpt::hwdrivers::INTEGRATED_ANGLE
@ INTEGRATED_ANGLE
Definition: CGyroKVHDSP3000.h:27
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:22
mrpt::hwdrivers::CGenericSensor::ssError
@ ssError
Definition: CGenericSensor.h:88
mrpt::hwdrivers::CGyroKVHDSP3000::m_COMbauds
int m_COMbauds
This serial port will be attempted to be opened automatically when this class is first used to reques...
Definition: CGyroKVHDSP3000.h:80
mrpt::comms
Serial and networking devices and utilities.
Definition: CClientTCPSocket.h:21
mrpt::comms::CSerialPort::Write
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
Definition: CSerialPort.cpp:847
mrpt::hwdrivers::CGenericSensor::m_process_rate
double m_process_rate
See CGenericSensor.
Definition: CGenericSensor.h:133
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::hwdrivers::CGenericSensor::m_state
TSensorState m_state
Definition: CGenericSensor.h:147
mrpt::system::tokenize
void tokenize(const std::string &inString, const std::string &inDelimiters, OUT_CONTAINER &outTokens, bool skipBlankTokens=true) noexcept
Tokenizes a string according to a set of delimiting characters.
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::config::CConfigFileBase::read_string
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:169
CGyroKVHDSP3000.h
mrpt::hwdrivers::CGyroKVHDSP3000::m_mode
GYRO_MODE m_mode
Definition: CGyroKVHDSP3000.h:91
mrpt::comms::CSerialPort::setConfig
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
Definition: CSerialPort.cpp:215
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::obs::CObservationIMU::Ptr
std::shared_ptr< CObservationIMU > Ptr
Definition: CObservationIMU.h:110
mrpt::hwdrivers::RATE
@ RATE
Definition: CGyroKVHDSP3000.h:25
mrpt::hwdrivers::CGyroKVHDSP3000::m_com_port
std::string m_com_port
Definition: CGyroKVHDSP3000.h:81
mrpt::hwdrivers::CGenericSensor::appendObservation
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
Definition: CGenericSensor.h:179
mrpt::hwdrivers::CGyroKVHDSP3000::changeMode
void changeMode(GYRO_MODE _newMode)
Definition: CGyroKVHDSP3000.cpp:156
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:140
mrpt::config::CConfigFileBase::read_float
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:113
CObservationIMU.h
IMPLEMENTS_GENERIC_SENSOR
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
Definition: CGenericSensor.h:330
mrpt::hwdrivers::GYRO_MODE
GYRO_MODE
Definition: CGyroKVHDSP3000.h:23
mrpt::comms::CSerialPort::ReadString
std::string ReadString(const int total_timeout_ms=-1, bool *out_timeout=nullptr, const char *eol_chars="\r\n")
Reads one text line from the serial port in POSIX "canonical mode".
Definition: CSerialPort.cpp:752
mrpt::hwdrivers::CGyroKVHDSP3000::m_firstInteration
bool m_firstInteration
Definition: CGyroKVHDSP3000.h:92
mrpt::obs::IMU_YAW
@ IMU_YAW
orientation yaw absolute value (global/navigation frame) (rad)
Definition: CObservationIMU.h:47
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::hwdrivers::CGenericSensor::ssInitializing
@ ssInitializing
Definition: CGenericSensor.h:86
mrpt::hwdrivers::CGyroKVHDSP3000
A class for interfacing KVH DSP 3000 gyroscope with an assynchronous serial communication (product SN...
Definition: CGyroKVHDSP3000.h:72
mrpt::obs::IMU_YAW_VEL
@ IMU_YAW_VEL
yaw angular velocity (local/vehicle frame) (rad/sec)
Definition: CObservationIMU.h:35
mrpt::hwdrivers::CGyroKVHDSP3000::~CGyroKVHDSP3000
virtual ~CGyroKVHDSP3000()
Destructor.
Definition: CGyroKVHDSP3000.cpp:43
mrpt::hwdrivers::CGyroKVHDSP3000::initialize
void initialize()
Turns on the KVH DSP 3000 device and configure it for getting orientation data.
Definition: CGyroKVHDSP3000.cpp:101
mrpt::hwdrivers::CGyroKVHDSP3000::loadConfig_sensorSpecific
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
Definition: CGyroKVHDSP3000.cpp:123
mrpt::hwdrivers::CGyroKVHDSP3000::resetIncrementalAngle
void resetIncrementalAngle(void)
Send to the sensor the command 'Z' wich reset the integrated angle.
Definition: CGyroKVHDSP3000.cpp:182
hwdrivers-precomp.h
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42



Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST