Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
12 #include <mrpt/system/threads.h>
15 
16 
17 
18 
20 
21 using namespace mrpt::utils;
22 using namespace mrpt::obs;
23 using namespace mrpt::hwdrivers;
24 using namespace std;
25 
26 /*-------------------------------------------------------------
27  CGyroKVHDSP3000
28 -------------------------------------------------------------*/
30  m_COMbauds (38400),
31  m_com_port (),
32  m_sensorPose (),
33  m_mode (RATE),
34  m_firstInteration(true)
35 {
36 
37  m_state = ssInitializing;
38  m_sensorLabel = "KVH_DSP3000";
39  m_serialPort = NULL;
40 
41 }
42 
43 /*-------------------------------------------------------------
44  ~CGyroKVHDSP3000
45 -------------------------------------------------------------*/
46 CGyroKVHDSP3000::~CGyroKVHDSP3000()
47 {
48 
50  delete m_serialPort;
51 
52 }
53 
54 /*-------------------------------------------------------------
55  doProcess
56 -------------------------------------------------------------*/
58 {
59 
60 
61  if(m_state == ssError)
62  {
64  initialize();
65  }
66 
67  if(m_state == ssError)
68  return;
69 
70  string msg;
71  CObservationIMUPtr observationGyro = CObservationIMU::Create();
72  observationGyro->timestamp = mrpt::system::now();
73 
74  msg = m_serialPort->ReadString(-1,NULL,"\n");
75 
76  observationGyro->sensorPose = m_sensorPose;
77  observationGyro->sensorLabel = m_sensorLabel;
78  string delimiter(" ");
79  vector<string> words;
80  mrpt::system::tokenize(msg, delimiter, words);
81  if(words.size() < 2) return;
82  if(words[1].c_str()[0] == '0') return;
83  double mesure = atof(words[0].c_str());
84  switch(m_mode)
85  {
86  case RATE :
87  observationGyro->rawMeasurements[IMU_YAW_VEL] = DEG2RAD(mesure);
88  observationGyro->dataIsPresent[IMU_YAW_VEL] = true;
89  break;
90  case INTEGRATED_ANGLE :
91  case INCREMENTAL_ANGLE :
92  observationGyro->rawMeasurements[IMU_YAW] = DEG2RAD(mesure);
93  observationGyro->dataIsPresent[IMU_YAW] = true;
94  break;
95  }
96 
98  {
99  appendObservation(observationGyro);
100  }
101  else m_firstInteration = false;
102 
103 
104 }
105 
106 /*-------------------------------------------------------------
107  initialize
108 -------------------------------------------------------------*/
110 {
111 
112  m_process_rate = 100;
113 
114  /*
115  Open modem device for reading and writing and not as controlling tty
116  because we don't want to get killed if linenoise sends CTRL-C.
117  */
118  if(m_serialPort) delete m_serialPort;
120  if(!(m_serialPort->isOpen())) THROW_EXCEPTION("can't open serial port");
121  cout<<"m_COMbaud "<<m_COMbauds<<endl;
123 
126  m_state = ssWorking;
127 
128 }
129 
130 /*-------------------------------------------------------------
131  loadConfig_sensorSpecific
132 -------------------------------------------------------------*/
134  const mrpt::utils::CConfigFileBase &configSource,
135  const std::string &iniSection )
136 {
137 
139  configSource.read_float( iniSection, "pose_x", 0, false ),
140  configSource.read_float( iniSection, "pose_y", 0, false ),
141  configSource.read_float( iniSection, "pose_z", 0, false ),
142  DEG2RAD( configSource.read_float( iniSection, "pose_yaw", 0, false ) ),
143  DEG2RAD( configSource.read_float( iniSection, "pose_pitch", 0, false ) ),
144  DEG2RAD( configSource.read_float( iniSection, "pose_roll", 0, false ) ) );
145  string operatingMode = configSource.read_string(iniSection, "operatingMode", "rate", false);
146  cout << "Operating mode : " << operatingMode << endl;
147  if(operatingMode == "incremental")
148  {
150  cout << "Incremental mode" << endl;
151  }
152  else if(operatingMode == "integral")
153  {
155  cout << "Integrated mode" << endl;
156  }
157  else
158  {
159  m_mode = RATE;
160  cout << "Rate mode" << endl;
161  }
162  m_com_port = configSource.read_string(iniSection, "COM_port_LIN", m_com_port, false );
163 
164 }
165 
167 {
168 
169  m_mode = _newMode;
170  char commande[3];
171  switch(m_mode)
172  {
173  case RATE :
174  commande[0] = 'R';
175  break;
176  case INTEGRATED_ANGLE :
177  commande[0] = 'P';
178  break;
179  case INCREMENTAL_ANGLE :
180  commande[0] = 'A'; // incremental.
181  break;
182  }
183  commande[1] = 0x0A; commande[2] = 0;
184  // we send the command four times to be sure that the command will be interpreted by the sensor.
185  if(m_serialPort->Write( commande, 3*sizeof(char)) <= 0)
186  {
187  THROW_EXCEPTION("can't write on serial port");
188  }
189 
190 }
191 
193 {
194 
195  if(m_mode != RATE)
196  {
197  char commande[3];
198  commande[0] = 'Z';
199  commande[1] = '\n';
200  commande[2] = 0;
201  if(m_serialPort->Write( commande, 3*sizeof(char)) <= 0)
202  {
203  THROW_EXCEPTION("can't write on serial port");
204  }
205  }
206 
207 }
208 
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
double m_process_rate
See CGenericSensor.
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
size_t Write(const void *Buffer, size_t Count)
Implements the virtual method responsible for writing to the stream.
void doProcess()
This method will be invoked at a minimum rate of "process_rate" (Hz) method must throw an exception w...
std::string ReadString(const int total_timeout_ms=-1, bool *out_timeout=NULL, const char *eol_chars="\r\n")
Reads one text line from the serial port in POSIX "canonical mode".
This class allows loading and storing values and vectors of different types from a configuration text...
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
void initialize()
Turns on the KVH DSP 3000 device and configure it for getting orientation data.
void BASE_IMPEXP sleep(int time_ms) MRPT_NO_THROWS
An OS-independent method for sending the current thread to "sleep" for a given period of time...
Definition: threads.cpp:57
void setConfig(int baudRate, int parity=0, int bits=8, int nStopBits=1, bool enableFlowControl=false)
Changes the configuration of the port.
#define DEG2RAD
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
void close()
Close the port.
void loadConfig_sensorSpecific(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
See the class documentation at the top for expected parameters.
void resetIncrementalAngle(void)
Send to the sensor the command 'Z' wich reset the integrated angle.
A class for interfacing KVH DSP 3000 gyroscope with an assynchronous serial communication (product SN...
bool isOpen() const
Returns if port has been correctly open.
mrpt::poses::CPose3D m_sensorPose
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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:254
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
orientation yaw absolute value (global/navigation frame) (rad)
yaw angular velocity (local/vehicle frame) (rad/sec)
void BASE_IMPEXP tokenize(const std::string &inString, const std::string &inDelimiters, std::deque< std::string > &outTokens, bool skipBlankTokens=true) MRPT_NO_THROWS
Tokenizes a string according to a set of delimiting characters.
CSerialPort * m_serialPort
The serial port connection.
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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018