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 }
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
double m_process_rate
See CGenericSensor.
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
mrpt::comms::CSerialPort * m_serialPort
Search the port where the sensor is located and connect to it.
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
std::string ReadString(const int total_timeout_ms=-1, bool *out_timeout=nullptr, const char *eol_chars="\")
Reads one text line from the serial port in POSIX "canonical mode".
virtual ~CGyroKVHDSP3000()
Destructor.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:75
Contains classes for various device interfaces.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
STL namespace.
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz)
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.
This class allows loading and storing values and vectors of different types from a configuration text...
void initialize()
Turns on the KVH DSP 3000 device and configure it for getting orientation data.
bool isOpen() const
Returns if port has been correctly open.
This namespace contains representation of robot actions and observations.
size_t Write(const void *Buffer, size_t Count) override
Introduces a pure virtual method responsible for writing to the stream.
GLsizei const GLchar ** string
Definition: glext.h:4101
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void close()
Close the port.
void resetIncrementalAngle(void)
Send to the sensor the command &#39;Z&#39; wich reset the integrated angle.
A class for interfacing KVH DSP 3000 gyroscope with an assynchronous serial communication (product SN...
mrpt::poses::CPose3D m_sensorPose
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
orientation yaw absolute value (global/navigation frame) (rad)
Serial and networking devices and utilities.
yaw angular velocity (local/vehicle frame) (rad/sec)
void changeMode(GYRO_MODE _newMode)
int m_COMbauds
This serial port will be attempted to be opened automatically when this class is first used to reques...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019