MRPT  1.9.9
CGyroKVHDSP3000.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, 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 
14 
15 #include <iostream>
16 #include <thread>
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() : m_com_port(), m_sensorPose()
29 
30 {
32  m_sensorLabel = "KVH_DSP3000";
33 }
34 
36 
37 /*-------------------------------------------------------------
38  doProcess
39 -------------------------------------------------------------*/
41 {
42  if (m_state == ssError)
43  {
44  std::this_thread::sleep_for(200ms);
45  initialize();
46  }
47 
48  if (m_state == ssError) return;
49 
50  string msg;
51  CObservationIMU::Ptr observationGyro = std::make_shared<CObservationIMU>();
52  observationGyro->timestamp = mrpt::system::now();
53 
54  msg = m_serialPort->ReadString(-1, nullptr, "\n");
55 
56  observationGyro->sensorPose = m_sensorPose;
57  observationGyro->sensorLabel = m_sensorLabel;
58  string delimiter(" ");
59  vector<string> words;
60  mrpt::system::tokenize(msg, delimiter, words);
61  if (words.size() < 2) return;
62  if (words[1].c_str()[0] == '0') return;
63  double mesure = atof(words[0].c_str());
64  switch (m_mode)
65  {
66  case RATE:
67  observationGyro->rawMeasurements[IMU_YAW_VEL] = DEG2RAD(mesure);
68  observationGyro->dataIsPresent[IMU_YAW_VEL] = true;
69  break;
70  case INTEGRATED_ANGLE:
71  case INCREMENTAL_ANGLE:
72  observationGyro->rawMeasurements[IMU_YAW] = DEG2RAD(mesure);
73  observationGyro->dataIsPresent[IMU_YAW] = true;
74  break;
75  }
76 
77  if (!m_firstInteration)
78  {
79  appendObservation(observationGyro);
80  }
81  else
82  m_firstInteration = false;
83 }
84 
85 /*-------------------------------------------------------------
86  initialize
87 -------------------------------------------------------------*/
89 {
90  m_process_rate = 100;
91 
92  /*
93  Open modem device for reading and writing and not as controlling tty
94  because we don't want to get killed if linenoise sends CTRL-C.
95  */
96  m_serialPort = std::make_unique<CSerialPort>(m_com_port);
97  if (!(m_serialPort->isOpen())) THROW_EXCEPTION("can't open serial port");
98  cout << "m_COMbaud " << m_COMbauds << endl;
99  m_serialPort->setConfig(m_COMbauds);
100 
103  m_state = ssWorking;
104 }
105 
106 /*-------------------------------------------------------------
107  loadConfig_sensorSpecific
108 -------------------------------------------------------------*/
110  const mrpt::config::CConfigFileBase& configSource,
111  const std::string& iniSection)
112 {
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 =
121  configSource.read_string(iniSection, "operatingMode", "rate", false);
122  cout << "Operating mode : " << operatingMode << endl;
123  if (operatingMode == "incremental")
124  {
126  cout << "Incremental mode" << endl;
127  }
128  else if (operatingMode == "integral")
129  {
131  cout << "Integrated mode" << endl;
132  }
133  else
134  {
135  m_mode = RATE;
136  cout << "Rate mode" << endl;
137  }
138  m_com_port =
139  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
140 }
141 
143 {
144  m_mode = _newMode;
145  char commande[3];
146  switch (m_mode)
147  {
148  case RATE:
149  commande[0] = 'R';
150  break;
151  case INTEGRATED_ANGLE:
152  commande[0] = 'P';
153  break;
154  case INCREMENTAL_ANGLE:
155  commande[0] = 'A'; // incremental.
156  break;
157  }
158  commande[1] = 0x0A;
159  commande[2] = 0;
160  // we send the command four times to be sure that the command will be
161  // interpreted by the sensor.
162  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
163  {
164  THROW_EXCEPTION("can't write on serial port");
165  }
166 }
167 
169 {
170  if (m_mode != RATE)
171  {
172  char commande[3];
173  commande[0] = 'Z';
174  commande[1] = '\n';
175  commande[2] = 0;
176  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
177  {
178  THROW_EXCEPTION("can't write on serial port");
179  }
180  }
181 }
void initialize() override
Turns on the KVH DSP 3000 device and configure it for getting orientation data.
void resetIncrementalAngle()
Send to the sensor the command &#39;Z&#39; wich reset the integrated angle.
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
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string m_sensorLabel
See CGenericSensor.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
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 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.
std::unique_ptr< mrpt::comms::CSerialPort > m_serialPort
Search the port where the sensor is located and connect to it.
This class allows loading and storing values and vectors of different types from a configuration text...
void doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
constexpr double DEG2RAD(const double x)
Degrees to radians.
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
This namespace contains representation of robot actions and observations.
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
A class for interfacing KVH DSP 3000 gyroscope with an assynchronous serial communication (product SN...
mrpt::poses::CPose3D m_sensorPose
~CGyroKVHDSP3000() override
Destructor.
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:265
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020