Main MRPT website > C++ reference for MRPT 1.5.6
CBoardENoses.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/os.h>
14 #include <mrpt/utils/CMessage.h>
15 #include <mrpt/math/ops_vectors.h>
16 
17 using namespace mrpt::utils;
18 using namespace mrpt::math;
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::hwdrivers;
22 using namespace std;
23 
25 
26 /*-------------------------------------------------------------
27  CBoardENoses
28 -------------------------------------------------------------*/
30  m_usbSerialNumber ("ENOSE001"),
31  m_COM_port (),
32  m_COM_baud (115200),
33  m_stream_FTDI (NULL),
34  m_stream_SERIAL (NULL)
35 {
36  m_sensorLabel = "ENOSE";
37  first_reading = true;
38 }
39 
40 CBoardENoses::~CBoardENoses( )
41 {
42  delete_safe(m_stream_FTDI);
43  delete_safe(m_stream_SERIAL);
44 }
45 
46 /*-------------------------------------------------------------
47  loadConfig_sensorSpecific
48 -------------------------------------------------------------*/
49 void CBoardENoses::loadConfig_sensorSpecific(
50  const mrpt::utils::CConfigFileBase &configSource,
51  const std::string &iniSection )
52 {
54 
55  m_usbSerialNumber = configSource.read_string(iniSection, "USB_serialname","",false);
56 
57 #ifdef MRPT_OS_WINDOWS
58  m_COM_port = configSource.read_string(iniSection, "COM_port_WIN",m_COM_port);
59 #else
60  m_COM_port = configSource.read_string(iniSection, "COM_port_LIN",m_COM_port);
61 #endif
62  m_COM_baud = configSource.read_uint64_t(iniSection, "COM_baudRate",m_COM_baud);
63 
64  configSource.read_vector( iniSection, "enose_poses_x", vector<float>(0), enose_poses_x, true);
65  configSource.read_vector( iniSection, "enose_poses_y", vector<float>(0), enose_poses_y, true);
66  configSource.read_vector( iniSection, "enose_poses_z", vector<float>(0), enose_poses_z, true);
67 
68  configSource.read_vector( iniSection, "enose_poses_yaw", vector<float>(0), enose_poses_yaw, true);
69  configSource.read_vector( iniSection, "enose_poses_pitch", vector<float>(0), enose_poses_pitch, true);
70  configSource.read_vector( iniSection, "enose_poses_roll", vector<float>(0), enose_poses_roll, true);
71 
72  ASSERT_( enose_poses_x.size() == enose_poses_y.size() );
73  ASSERT_( enose_poses_x.size() == enose_poses_z.size() );
74  ASSERT_( enose_poses_x.size() == enose_poses_yaw.size() );
75  ASSERT_( enose_poses_x.size() == enose_poses_pitch.size() );
76  ASSERT_( enose_poses_x.size() == enose_poses_roll.size() );
77 
78  // Pass angles to radians:
79  enose_poses_yaw *= M_PIf / 180.0f;
80  enose_poses_pitch *= M_PIf / 180.0f;
81  enose_poses_roll *= M_PIf / 180.0f;
82 
83  MRPT_END
84 
85 }
86 
87 /*-------------------------------------------------------------
88  queryFirmwareVersion
89 -------------------------------------------------------------*/
90 bool CBoardENoses::queryFirmwareVersion( string &out_firmwareVersion )
91 {
92  try
93  {
94  mrpt::utils::CMessage msg,msgRx;
95 
96  // Try to connect to the device:
97  CStream *comms = checkConnectionAndConnect();
98  if (!comms)
99  return false;
100 
101  msg.type = 0x10;
102  comms->sendMessage(msg);
103 
104  if (comms->receiveMessage(msgRx) )
105  {
106  msgRx.getContentAsString( out_firmwareVersion );
107  return true;
108  }
109  else
110  return false;
111  }
112  catch(...)
113  {
114  // Close everything and start again:
115  delete_safe(m_stream_SERIAL);
116  delete_safe(m_stream_FTDI);
117  return false;
118  }
119 }
120 
121 
122 /*-------------------------------------------------------------
123  checkConnectionAndConnect
124 -------------------------------------------------------------*/
125 CStream *CBoardENoses::checkConnectionAndConnect()
126 {
127  // Make sure one of the two possible pipes is open:
128  if (!m_stream_FTDI && !m_stream_SERIAL)
129  {
130  if (!m_COM_port.empty())
131  m_stream_SERIAL = new CSerialPort();
132  else m_stream_FTDI = new CInterfaceFTDI();
133  }
134 
135 
136  if (m_stream_FTDI)
137  { // FTDI pipe ==================
138  if (m_stream_FTDI->isOpen())
139  return m_stream_FTDI;
140  try
141  {
142  m_stream_FTDI->OpenBySerialNumber( m_usbSerialNumber );
144  m_stream_FTDI->Purge();
146  m_stream_FTDI->SetLatencyTimer(1);
147  m_stream_FTDI->SetTimeouts(10,100);
148  return m_stream_FTDI;
149  }
150  catch(...)
151  { // Error opening device:
152  m_stream_FTDI->Close();
153  return NULL;
154  }
155  }
156  else
157  { // Serial pipe ==================
158  ASSERT_(m_stream_SERIAL)
159  if (m_stream_SERIAL->isOpen())
160  return m_stream_SERIAL;
161  try
162  {
163  m_stream_SERIAL->open(m_COM_port);
164  m_stream_SERIAL->setConfig(m_COM_baud);
166  m_stream_SERIAL->purgeBuffers();
168  //m_stream_SERIAL->setTimeouts(25,1,100, 1,20);
169  m_stream_SERIAL->setTimeouts(50,1,100, 1,20);
170  return m_stream_SERIAL;
171  }
172  catch(...)
173  { // Error opening device:
174  m_stream_SERIAL->close();
175  return NULL;
176  }
177  }
178 }
179 
180 
181 /*-------------------------------------------------------------
182  getObservation
183 -------------------------------------------------------------*/
185 {
186  try
187  {
188  // Connected?
189  CStream *comms = checkConnectionAndConnect();
190 
191  if (!comms)
192  return false;
193 
194  utils::CMessage msg;
196 
197  obs.m_readings.clear();
198 
199  //// Send request:
200  //msg.type = 0x11;
201  //msg.content.clear();
202  //comms->sendMessage( msg );
203 
204  //----------------------------MCE-nose FRAME--------------------------------------------------
205  // Wait for e-nose frame: <0x69><0x91><lenght><body><0x96> "Bytes"
206  // Where <body> = [Numchamber, Activechamber, N sensors*M chambers*2, 2 timestamp] of uint16_t
207  // MCE-nose provides a 136B body lenght which makes 140B total frame lenght
208 
209  if (!comms->receiveMessage( msg ))
210  {
211  return false;
212  }
213 
214  //m_state = ssWorking;
215 
216  // Copy to "uint16_t":
217  ASSERT_((msg.content.size() % 2)==0);
218 
219  vector<uint16_t> readings( msg.content.size() / 2 ); // divide by 2 to pass from byte to word. 136B/2 = 68 Words
220 
221  if (msg.content.size()>0)
222  {
223  // Copy to a vector of 16bit integers:
224  memcpy( &readings[0],&msg.content[0],msg.content.size() * sizeof(msg.content[0]) );
225 
226  //HEADER Frame [ NÂș of chambers/enoses (16b) , Active Chamber (16b)]
227  size_t NumberOfChambers = (size_t)readings[0];
228  size_t ActiveChamber = (size_t)readings[1];
229 
230  // Sensors readings info
231  ASSERT_( ((readings.size() - 4) % NumberOfChambers)==0 );
232  size_t wordsPereNose = (readings.size() - 4) / NumberOfChambers;
233 
234 
235  // Process each chamber
236  for (size_t i=0; i<NumberOfChambers; i++)
237  {
238  // ----------------------------------------------------------------------
239  // Each "i" comprises a complete Enose reading: Gas sensors + temperature
240  // ----------------------------------------------------------------------
241 
242  // Do we have the sensor position?
243  if (i<enose_poses_x.size())
244  {
245  newRead.eNosePoseOnTheRobot = CPose3D(
246  enose_poses_x[i],
247  enose_poses_y[i],
248  enose_poses_z[i],
249  enose_poses_yaw[i],
250  enose_poses_pitch[i],
251  enose_poses_roll[i]);
252  }
253  else
254  newRead.eNosePoseOnTheRobot = CPose3D(0,0,0);
255 
256  // Process the sensor codes:
257  newRead.sensorTypes.clear();
258  newRead.readingsVoltage.clear();
259  newRead.hasTemperature = false;
260  newRead.isActive = false;
261 
262  // check if active chamber
263  if (i == (ActiveChamber))
264  newRead.isActive = true;
265 
266  //process each sensor on this chamber "i"
267  for (size_t idx=0 ; idx<wordsPereNose/2 ; idx++)
268  {
269  if ( readings[i*wordsPereNose + 2*idx + 2] != 0x0000 ) //not empty slot
270  {
271  // Is temperature?
272  if (readings[i*wordsPereNose + 2*idx + 2] == 0xFFFF)
273  {
274  newRead.hasTemperature = true;
275  newRead.temperature = ((int16_t)readings[i*wordsPereNose + 2*idx + 3]) / 32.0f;
276  }
277  else //Is a gas sensors
278  {
279  // It is not a null code: There is a valid measure:
280  newRead.sensorTypes.push_back( readings[i*wordsPereNose + 2*idx + 2] );
281 
282  // Pass from ADC value[12bits] to [0-2.5] volt range:
283  newRead.readingsVoltage.push_back( ( readings[i*wordsPereNose + 2*idx + 3] * 5.0f) / 4096.0f );
284  }
285  }
286  } // end for each sensor on this eNose
287 
288  // Add to observations:
289  if ( !newRead.sensorTypes.empty() )
290  obs.m_readings.push_back( newRead );
291 
292  } // end for each i'th eNose
293 
294  obs.sensorLabel = m_sensorLabel;
295 
296  // Set Timestamp
297  uint16_t* p =
298  (uint16_t*)&readings[readings.size() - 2]; // Get readings time
299  // from frame
300  // (always last 2
301  // words)
302  obs.timestamp =
303  mrpt::system::secondsToTimestamp(((double)*p) / 1000);
304 
305  if (first_reading)
306  {
307  initial_timestamp = mrpt::system::getCurrentTime() - obs.timestamp;
308  first_reading = false;
309  }
310  obs.timestamp = obs.timestamp + initial_timestamp;
311 
312  } // end if message has data
313 
314 
315  //CONTROL
316  bool correct = true;
317 
318  if (obs.m_readings.size() != 4)
319  correct = false;
320  else
321  {
322  for (size_t ch=0; ch<obs.m_readings.size(); ch++)
323  {
324  if( (obs.m_readings[ch].sensorTypes.size() != 7) || (obs.m_readings[ch].readingsVoltage.size() != 7) )
325  correct = false;
326  else
327  {
328 
329  }
330  }
331  }
332 
333  if (!correct)
334  printf("Error en la observacion"); //For debug
335 
336 
337  return !obs.m_readings.empty(); // Done OK!
338  }
339  catch(exception &e)
340  {
341  cerr << "[CBoardENoses::getObservation] Returning false due to exception: " << endl;
342  cerr << e.what() << endl;
343  return false;
344  }
345  catch(...)
346  {
347  return false;
348  }
349 }
350 
351 
352 
353 /*-------------------------------------------------------------
354  doProcess
355 -------------------------------------------------------------*/
356 /** This method should be called periodically (at least at 1Hz to capture ALL the real-time data)
357 * It is thread safe, i.e. you can call this from one thread, then to other methods from other threads.
358 */
359 void CBoardENoses::doProcess()
360 {
361  CObservationGasSensorsPtr obs= CObservationGasSensors::Create();
362 
363  if (getObservation(*obs))
364  {
365  m_state = ssWorking;
366  appendObservation( obs );
367  }
368  else
369  {
370  m_state = ssError;
371  // THROW_EXCEPTION("No observation received from the USB board!");
372  }
373 }
374 
375 
376 /*-------------------------------------------------------------
377  initialize
378 -------------------------------------------------------------*/
379 /** Tries to open the camera, after setting all the parameters with a call to loadConfig.
380  * \exception This method must throw an exception with a descriptive message if some critical error is found.
381  */
382 void CBoardENoses::initialize()
383 {
384  // We'll rather try it in doProcess() since it's quite usual that it fails
385  // on a first try, then works on the next ones.
386  /*
387  if (!checkConnectionAndConnect())
388  THROW_EXCEPTION("Couldn't connect to the eNose board");
389  */
390 }
391 
392 /*-------------------------------------------------------------
393  setActiveChamber
394 -------------------------------------------------------------*/
395 /** Send to the MCE-nose the next Active Chamber */
396 
397 bool CBoardENoses::setActiveChamber( unsigned char chamber )
398 {
399  try
400  {
401  // Try to connect to the device:
402  CStream *comms = checkConnectionAndConnect();
403  if (!comms)
404  return false;
405 
406  // Send a byte to set the Active chamber on device.
407  // by default: Byte_to_send = 10_ _ _ _10
408  unsigned char buf[1];
409  buf[0] = ((chamber & 15) << 2) | 130; //130= 10 0000 10
410 
411  comms->WriteBuffer(buf,1); // Exceptions will be raised on errors here
412  return true;
413  }
414  catch(...)
415  {
416  // Close everything and start again:
417  delete_safe(m_stream_SERIAL);
418  delete_safe(m_stream_FTDI);
419  return false;
420  }
421 }
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
unsigned __int16 uint16_t
Definition: rptypes.h:46
OBSERVATION_T::Ptr getObservation(mrpt::obs::CSensoryFramePtr &observations, mrpt::obs::CObservationPtr &observation, bool priority_to_sf=true)
Given an mrpt::obs::CSensoryFrame and a mrpt::obs::CObservation pointer if a OBSERVATION_T type obser...
Definition: obs_utils.h:32
A communications serial port built as an implementation of a utils::CStream.
Definition: CSerialPort.h:43
mrpt::system::TTimeStamp BASE_IMPEXP getCurrentTime()
Returns the current (UTC) system time.
Definition: datetime.cpp:71
float temperature
Sensed temperature in Celcius (valid if hasTemperature=true only)
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
vector_int sensorTypes
The kind of sensors in the array (size of "sensorTypes" is the same that the size of "readingsVoltage...
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
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL...
Definition: bits.h:196
#define M_PIf
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
__int16 int16_t
Definition: rptypes.h:45
#define MRPT_END
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
A class for interfacing an e-Noses via a FTDI USB link.
Definition: CBoardENoses.h:53
bool hasTemperature
Must be true for "temperature" to contain a valid measurement.
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
GLfloat GLfloat p
Definition: glew.h:10113
#define IMPLEMENTS_GENERIC_SENSOR(class_name, NameSpace)
This must be inserted in all CGenericSensor classes implementation files:
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
bool isActive
True if the input to this chamber/enose is poluted air, False if clean air.
void getContentAsString(std::string &str)
Gets the contents of the message as a string.
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
bool receiveMessage(utils::CMessage &msg)
Tries to receive a message from the device.
Definition: CStream.cpp:590
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
std::vector< TObservationENose > m_readings
One entry per e-nose on the robot.
#define ASSERT_(f)
void sendMessage(const utils::CMessage &msg)
Send a message to the device.
Definition: CStream.cpp:556
std::vector< uint8_t > content
The contents of the message (memory is automatically handled by the std::vector object) ...
Definition: CMessage.h:33
uint64_t read_uint64_t(const std::string &section, const std::string &name, uint64_t defaultValue, bool failIfNotFound=false) const
mrpt::system::TTimeStamp BASE_IMPEXP secondsToTimestamp(const double nSeconds)
Transform a time interval (in seconds) into TTimeStamp (e.g.
Definition: datetime.cpp:219
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
A definition of a CStream actually representing a USB connection to a FTDI chip.
A class that contain generic messages, that can be sent and received from a "CClientTCPSocket" object...
Definition: CMessage.h:29
uint32_t type
An identifier of the message type (only the least-sig byte is typically sent)
Definition: CMessage.h:32
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



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