Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
vpComedi.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * ATI Force torque interface.
33  *
34 *****************************************************************************/
35 
36 #include <visp3/core/vpConfig.h>
37 
38 #ifdef VISP_HAVE_COMEDI
39 
40 #include <unistd.h>
41 
42 #include <visp3/core/vpException.h>
43 #include <visp3/sensor/vpComedi.h>
44 
45 BEGIN_VISP_NAMESPACE
50  : m_device("/dev/comedi0"), m_handler(nullptr), m_subdevice(0), m_range(0), m_aref(AREF_DIFF), m_nchannel(6),
51  m_range_info(6), m_maxdata(6), m_chanlist(6)
52 { }
53 
59 
64 {
65  if (!m_handler) {
66  m_handler = comedi_open(m_device.c_str());
67 
68  if (!m_handler) {
69  throw vpException(vpException::fatalError, "Could not open device %s", m_device.c_str());
70  }
71 
72  // Print NaN for clipped inputs
73  comedi_set_global_oor_behavior(COMEDI_OOR_NAN);
74 
75  // Setup data range and max value
76  m_range_info.resize(m_nchannel);
77  m_maxdata.resize(m_nchannel);
78  m_chanlist.resize(m_nchannel);
79  for (unsigned int channel = 0; channel < m_nchannel; channel++) {
80  m_chanlist[channel] = CR_PACK(channel, m_range, m_aref);
81  m_range_info[channel] = comedi_get_range(m_handler, m_subdevice, channel, m_range);
82  m_maxdata[channel] = comedi_get_maxdata(m_handler, m_subdevice, channel);
83  }
84  }
85 }
86 
91 {
92  if (m_handler) {
93  comedi_close(m_handler);
94  m_handler = nullptr;
95  }
96 }
97 
105 std::vector<lsampl_t> vpComedi::getRawData() const
106 {
107  if (m_handler == nullptr) {
108  throw vpException(vpException::fatalError, "Comedi device not open");
109  }
110  // Get raw data
111  std::vector<lsampl_t> raw_data(m_nchannel);
112 
113  for (unsigned int channel = 0; channel < m_nchannel; channel++) {
114  // When switching the multiplexor from one channel to the next, the A/D
115  // input needs time to settle to the new input voltage. The greater the
116  // voltage difference, the more time it takes. Here we wait for 1us
117  int ret = comedi_data_read_delayed(m_handler, m_subdevice, channel, m_range, m_aref, &raw_data[channel], 1000);
118  if (ret < 0) {
120  "Cannot get %d data from device=%s subdevice=%d "
121  "channel=%d range=%d analog reference=%d",
122  m_nchannel, m_device.c_str(), m_subdevice, channel, m_aref);
123  }
124  }
125 
126  return raw_data;
127 }
128 
134 {
135  if (m_handler == nullptr) {
136  throw vpException(vpException::fatalError, "Comedi device not open");
137  }
138  // Get raw data
139  std::vector<lsampl_t> raw_data = this->getRawData();
140  vpColVector phy_data(m_nchannel);
141 
142  // Convert data to physical data
143  for (unsigned int channel = 0; channel < m_nchannel; channel++) {
144  phy_data[channel] = comedi_to_phys(raw_data[channel], m_range_info[channel], m_maxdata[channel]);
145  if (vpMath::isNaN(phy_data[channel])) {
146  throw vpException(vpException::fatalError, "Comedi DAQ get NaN value. Check the connection with your device");
147  }
148  }
149 
150  return phy_data;
151 }
152 
155 std::string vpComedi::getPhyDataUnits() const
156 {
157  if (m_handler == nullptr) {
158  throw vpException(vpException::fatalError, "Comedi device not open");
159  }
160  std::string units;
161  unsigned int channel = 0;
162  switch (m_range_info[channel]->unit) {
163  case UNIT_volt:
164  units = "V";
165  break;
166  case UNIT_mA:
167  units = "mA";
168  break;
169  case UNIT_none:
170  break;
171  }
172  return units;
173 }
174 END_VISP_NAMESPACE
175 #elif !defined(VISP_BUILD_SHARED_LIBS)
176 // Work around to avoid warning: libvisp_sensor.a(vpComedi.cpp.o) has symbols
177 void dummy_vpComedi() { };
178 #endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
vpComedi()
Definition: vpComedi.cpp:49
virtual ~vpComedi()
Definition: vpComedi.cpp:58
unsigned int m_aref
Definition: vpComedi.h:175
std::vector< lsampl_t > m_maxdata
Definition: vpComedi.h:178
std::string m_device
Definition: vpComedi.h:171
std::vector< lsampl_t > getRawData() const
Definition: vpComedi.cpp:105
unsigned int m_range
Definition: vpComedi.h:174
vpColVector getPhyData() const
Definition: vpComedi.cpp:133
void open()
Definition: vpComedi.cpp:63
std::vector< comedi_range * > m_range_info
Definition: vpComedi.h:177
unsigned int m_subdevice
Definition: vpComedi.h:173
comedi_t * m_handler
Definition: vpComedi.h:172
unsigned int m_nchannel
Definition: vpComedi.h:176
std::vector< unsigned int > m_chanlist
Definition: vpComedi.h:179
std::string getPhyDataUnits() const
Definition: vpComedi.cpp:155
void close()
Definition: vpComedi.cpp:90
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ fatalError
Fatal error.
Definition: vpException.h:72
static bool isNaN(double value)
Definition: vpMath.cpp:92