Visual Servoing Platform version 3.5.0
vpComedi.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Authors:
35 * Fabien Spindler
36 *
37 *****************************************************************************/
38
39#include <visp3/core/vpConfig.h>
40
41#ifdef VISP_HAVE_COMEDI
42
43#include <unistd.h>
44
45#include <visp3/core/vpException.h>
46#include <visp3/sensor/vpComedi.h>
47
52 : m_device("/dev/comedi0"), m_handler(NULL), m_subdevice(0), m_range(0), m_aref(AREF_DIFF), m_nchannel(6),
53 m_range_info(6), m_maxdata(6), m_chanlist(6)
54{
55}
56
62
67{
68 if (!m_handler) {
69 m_handler = comedi_open(m_device.c_str());
70
71 if (!m_handler) {
72 throw vpException(vpException::fatalError, "Could not open device %s", m_device.c_str());
73 }
74
75 // Print NaN for clipped inputs
76 comedi_set_global_oor_behavior(COMEDI_OOR_NAN);
77
78 // Setup data range and max value
80 m_maxdata.resize(m_nchannel);
81 m_chanlist.resize(m_nchannel);
82 for (unsigned int channel = 0; channel < m_nchannel; channel++) {
83 m_chanlist[channel] = CR_PACK(channel, m_range, m_aref);
84 m_range_info[channel] = comedi_get_range(m_handler, m_subdevice, channel, m_range);
85 m_maxdata[channel] = comedi_get_maxdata(m_handler, m_subdevice, channel);
86 }
87 }
88}
89
94{
95 if (m_handler) {
96 comedi_close(m_handler);
97 m_handler = NULL;
98 }
99}
100
108std::vector<lsampl_t> vpComedi::getRawData() const
109{
110 if (m_handler == NULL) {
111 throw vpException(vpException::fatalError, "Comedi device not open");
112 }
113 // Get raw data
114 std::vector<lsampl_t> raw_data(m_nchannel);
115
116 for (unsigned int channel = 0; channel < m_nchannel; channel++) {
117 // When switching the multiplexor from one channel to the next, the A/D
118 // input needs time to settle to the new input voltage. The greater the
119 // voltage difference, the more time it takes. Here we wait for 1us
120 int ret = comedi_data_read_delayed(m_handler, m_subdevice, channel, m_range, m_aref, &raw_data[channel], 1000);
121 if (ret < 0) {
123 "Cannot get %d data from device=%s subdevice=%d "
124 "channel=%d range=%d analog reference=%d",
125 m_nchannel, m_device.c_str(), m_subdevice, channel, m_aref);
126 }
127 }
128
129 return raw_data;
130}
131
137{
138 if (m_handler == NULL) {
139 throw vpException(vpException::fatalError, "Comedi device not open");
140 }
141 // Get raw data
142 std::vector<lsampl_t> raw_data = this->getRawData();
143 vpColVector phy_data(m_nchannel);
144
145 // Convert data to physical data
146 for (unsigned int channel = 0; channel < m_nchannel; channel++) {
147 phy_data[channel] = comedi_to_phys(raw_data[channel], m_range_info[channel], m_maxdata[channel]);
148 if (vpMath::isNaN(phy_data[channel])) {
149 throw vpException(vpException::fatalError, "Comedi DAQ get NaN value. Check the connection with your device");
150 }
151 }
152
153 return phy_data;
154}
155
158std::string vpComedi::getPhyDataUnits() const
159{
160 if (m_handler == NULL) {
161 throw vpException(vpException::fatalError, "Comedi device not open");
162 }
163 std::string units;
164 unsigned int channel = 0;
165 switch (m_range_info[channel]->unit) {
166 case UNIT_volt:
167 units = "V";
168 break;
169 case UNIT_mA:
170 units = "mA";
171 break;
172 case UNIT_none:
173 break;
174 }
175 return units;
176}
177
178#elif !defined(VISP_BUILD_SHARED_LIBS)
179// Work arround to avoid warning: libvisp_sensor.a(vpComedi.cpp.o) has no
180// symbols
181void dummy_vpComedi(){};
182#endif
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
virtual ~vpComedi()
Definition: vpComedi.cpp:61
unsigned int m_aref
Definition: vpComedi.h:173
std::vector< lsampl_t > m_maxdata
Definition: vpComedi.h:176
std::string m_device
Definition: vpComedi.h:169
std::vector< lsampl_t > getRawData() const
Definition: vpComedi.cpp:108
unsigned int m_range
Definition: vpComedi.h:172
vpColVector getPhyData() const
Definition: vpComedi.cpp:136
void open()
Definition: vpComedi.cpp:66
std::vector< comedi_range * > m_range_info
Definition: vpComedi.h:175
vpComedi()
Definition: vpComedi.cpp:51
unsigned int m_subdevice
Definition: vpComedi.h:171
comedi_t * m_handler
Definition: vpComedi.h:170
unsigned int m_nchannel
Definition: vpComedi.h:174
std::vector< unsigned int > m_chanlist
Definition: vpComedi.h:177
std::string getPhyDataUnits() const
Definition: vpComedi.cpp:158
void close()
Definition: vpComedi.cpp:93
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static bool isNaN(double value)
Definition: vpMath.cpp:85