Visual Servoing Platform
version 3.6.0
Loading...
Searching...
No Matches
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
48
vpComedi::vpComedi
()
49
: m_device(
"/dev/comedi0"
), m_handler(NULL), m_subdevice(0), m_range(0), m_aref(AREF_DIFF), m_nchannel(6),
50
m_range_info(6), m_maxdata(6), m_chanlist(6)
51
{
52
}
53
58
vpComedi::~vpComedi
() {
close
(); }
59
63
void
vpComedi::open
()
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
90
void
vpComedi::close
()
91
{
92
if
(
m_handler
) {
93
comedi_close(
m_handler
);
94
m_handler
= NULL;
95
}
96
}
97
105
std::vector<lsampl_t>
vpComedi::getRawData
()
const
106
{
107
if
(
m_handler
== NULL) {
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) {
119
throw
vpException
(
vpException::fatalError
,
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
133
vpColVector
vpComedi::getPhyData
()
const
134
{
135
if
(
m_handler
== NULL) {
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
== NULL) {
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
175
#elif !defined(VISP_BUILD_SHARED_LIBS)
176
// Work around to avoid warning: libvisp_sensor.a(vpComedi.cpp.o) has no
177
// symbols
178
void
dummy_vpComedi(){};
179
#endif
vpColVector
Implementation of column vector and the associated operations.
Definition
vpColVector.h:167
vpComedi::~vpComedi
virtual ~vpComedi()
Definition
vpComedi.cpp:58
vpComedi::m_aref
unsigned int m_aref
Definition
vpComedi.h:170
vpComedi::m_maxdata
std::vector< lsampl_t > m_maxdata
Definition
vpComedi.h:173
vpComedi::m_device
std::string m_device
Definition
vpComedi.h:166
vpComedi::getRawData
std::vector< lsampl_t > getRawData() const
Definition
vpComedi.cpp:105
vpComedi::m_range
unsigned int m_range
Definition
vpComedi.h:169
vpComedi::getPhyData
vpColVector getPhyData() const
Definition
vpComedi.cpp:133
vpComedi::open
void open()
Definition
vpComedi.cpp:63
vpComedi::m_range_info
std::vector< comedi_range * > m_range_info
Definition
vpComedi.h:172
vpComedi::vpComedi
vpComedi()
Definition
vpComedi.cpp:48
vpComedi::m_subdevice
unsigned int m_subdevice
Definition
vpComedi.h:168
vpComedi::m_handler
comedi_t * m_handler
Definition
vpComedi.h:167
vpComedi::m_nchannel
unsigned int m_nchannel
Definition
vpComedi.h:171
vpComedi::m_chanlist
std::vector< unsigned int > m_chanlist
Definition
vpComedi.h:174
vpComedi::getPhyDataUnits
std::string getPhyDataUnits() const
Definition
vpComedi.cpp:155
vpComedi::close
void close()
Definition
vpComedi.cpp:90
vpException
error that can be emitted by ViSP classes.
Definition
vpException.h:59
vpException::fatalError
@ fatalError
Fatal error.
Definition
vpException.h:84
vpMath::isNaN
static bool isNaN(double value)
Definition
vpMath.cpp:93
modules
sensor
src
force-torque
vpComedi.cpp
Generated by
1.9.8