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-2019, 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  m_serialPort = nullptr;
34 }
35 
36 /*-------------------------------------------------------------
37  ~CGyroKVHDSP3000
38 -------------------------------------------------------------*/
40 {
42  delete m_serialPort;
43 }
44 
45 /*-------------------------------------------------------------
46  doProcess
47 -------------------------------------------------------------*/
49 {
50  if (m_state == ssError)
51  {
52  std::this_thread::sleep_for(200ms);
53  initialize();
54  }
55 
56  if (m_state == ssError) return;
57 
58  string msg;
59  CObservationIMU::Ptr observationGyro = std::make_shared<CObservationIMU>();
60  observationGyro->timestamp = mrpt::system::now();
61 
62  msg = m_serialPort->ReadString(-1, nullptr, "\n");
63 
64  observationGyro->sensorPose = m_sensorPose;
65  observationGyro->sensorLabel = m_sensorLabel;
66  string delimiter(" ");
67  vector<string> words;
68  mrpt::system::tokenize(msg, delimiter, words);
69  if (words.size() < 2) return;
70  if (words[1].c_str()[0] == '0') return;
71  double mesure = atof(words[0].c_str());
72  switch (m_mode)
73  {
74  case RATE:
75  observationGyro->rawMeasurements[IMU_YAW_VEL] = DEG2RAD(mesure);
76  observationGyro->dataIsPresent[IMU_YAW_VEL] = true;
77  break;
78  case INTEGRATED_ANGLE:
79  case INCREMENTAL_ANGLE:
80  observationGyro->rawMeasurements[IMU_YAW] = DEG2RAD(mesure);
81  observationGyro->dataIsPresent[IMU_YAW] = true;
82  break;
83  }
84 
85  if (!m_firstInteration)
86  {
87  appendObservation(observationGyro);
88  }
89  else
90  m_firstInteration = false;
91 }
92 
93 /*-------------------------------------------------------------
94  initialize
95 -------------------------------------------------------------*/
97 {
98  m_process_rate = 100;
99 
100  /*
101  Open modem device for reading and writing and not as controlling tty
102  because we don't want to get killed if linenoise sends CTRL-C.
103  */
104  if (m_serialPort) delete m_serialPort;
106  if (!(m_serialPort->isOpen())) THROW_EXCEPTION("can't open serial port");
107  cout << "m_COMbaud " << m_COMbauds << endl;
109 
112  m_state = ssWorking;
113 }
114 
115 /*-------------------------------------------------------------
116  loadConfig_sensorSpecific
117 -------------------------------------------------------------*/
119  const mrpt::config::CConfigFileBase& configSource,
120  const std::string& iniSection)
121 {
123  configSource.read_float(iniSection, "pose_x", 0, false),
124  configSource.read_float(iniSection, "pose_y", 0, false),
125  configSource.read_float(iniSection, "pose_z", 0, false),
126  DEG2RAD(configSource.read_float(iniSection, "pose_yaw", 0, false)),
127  DEG2RAD(configSource.read_float(iniSection, "pose_pitch", 0, false)),
128  DEG2RAD(configSource.read_float(iniSection, "pose_roll", 0, false)));
129  string operatingMode =
130  configSource.read_string(iniSection, "operatingMode", "rate", false);
131  cout << "Operating mode : " << operatingMode << endl;
132  if (operatingMode == "incremental")
133  {
135  cout << "Incremental mode" << endl;
136  }
137  else if (operatingMode == "integral")
138  {
140  cout << "Integrated mode" << endl;
141  }
142  else
143  {
144  m_mode = RATE;
145  cout << "Rate mode" << endl;
146  }
147  m_com_port =
148  configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false);
149 }
150 
152 {
153  m_mode = _newMode;
154  char commande[3];
155  switch (m_mode)
156  {
157  case RATE:
158  commande[0] = 'R';
159  break;
160  case INTEGRATED_ANGLE:
161  commande[0] = 'P';
162  break;
163  case INCREMENTAL_ANGLE:
164  commande[0] = 'A'; // incremental.
165  break;
166  }
167  commande[1] = 0x0A;
168  commande[2] = 0;
169  // we send the command four times to be sure that the command will be
170  // interpreted by the sensor.
171  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
172  {
173  THROW_EXCEPTION("can't write on serial port");
174  }
175 }
176 
178 {
179  if (m_mode != RATE)
180  {
181  char commande[3];
182  commande[0] = 'Z';
183  commande[1] = '\n';
184  commande[2] = 0;
185  if (m_serialPort->Write(commande, 3 * sizeof(char)) <= 0)
186  {
187  THROW_EXCEPTION("can't write on serial port");
188  }
189  }
190 }
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
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:41
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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".
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 setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
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 doProcess() override
This method will be invoked at a minimum rate of "process_rate" (Hz)
void loadConfig_sensorSpecific(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection) override
See the class documentation at the top for expected parameters.
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:4116
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void close()
Close the port.
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:256
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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at dom jul 14 20:00:11 CEST 2019