Debian Codegraph @

libs/hwdrivers/src/CNationalInstrumentsDAQ.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/hwdrivers/CNationalInstrumentsDAQ.h>
13 #include <mrpt/serialization/CArchive.h>
14
15 #include <chrono>
16 #include <iostream>
17 #include <iterator> // advance()
18 #include <thread>
19
20 // If we have both, DAQmx & DAQmxBase, prefer DAQmx:
21 #define MRPT_HAS_SOME_NIDAQMX (MRPT_HAS_NIDAQMXBASE || MRPT_HAS_NIDAQMX)
22
23 #define MRPT_USE_NIDAQMXBASE (MRPT_HAS_NIDAQMXBASE && !MRPT_HAS_NIDAQMX)
24 #define MRPT_USE_NIDAQMX (MRPT_HAS_NIDAQMX)
25
26 #if MRPT_USE_NIDAQMXBASE
27 #include "NIDAQmxBase.h" // Include file for NI-DAQmx Base API
28 #endif
29 #if MRPT_USE_NIDAQMX
30 #include "NIDAQmx.h" // Include file for NI-DAQmx API
31 #endif
32
33 // Macros to use either DAQmx or DAQmx Base automatically, depending on the
34 // installed libraries:
35 #if MRPT_USE_NIDAQMXBASE
36 #define MRPT_DAQmxGetExtendedErrorInfo DAQmxBaseGetExtendedErrorInfo
37 #define MRPT_DAQmxCreateTask DAQmxBaseCreateTask
38 #define MRPT_DAQmxCreateAIVoltageChan DAQmxBaseCreateAIVoltageChan
39 #define MRPT_DAQmxCreateAOVoltageChan DAQmxBaseCreateAOVoltageChan
40 #define MRPT_DAQmxCreateDIChan DAQmxBaseCreateDIChan
41 #define MRPT_DAQmxCreateDOChan DAQmxBaseCreateDOChan
42 #define MRPT_DAQmxCreateCIPeriodChan DAQmxBaseCreateCIPeriodChan
43 #define MRPT_DAQmxCreateCICountEdgesChan DAQmxBaseCreateCICountEdgesChan
44 #define MRPT_DAQmxCreateCIPulseWidthChan DAQmxBaseCreateCIPulseWidthChan
45 #define MRPT_DAQmxCreateCILinEncoderChan DAQmxBaseCreateCILinEncoderChan
46 #define MRPT_DAQmxCreateCIAngEncoderChan DAQmxBaseCreateCIAngEncoderChan
47 #define MRPT_DAQmxCreateCOPulseChanFreq DAQmxBaseCreateCOPulseChanFreq
48 #define MRPT_DAQmxCfgSampClkTiming DAQmxBaseCfgSampClkTiming
49 #define MRPT_DAQmxCfgInputBuffer DAQmxBaseCfgInputBuffer
50 #define MRPT_DAQmxCfgOutputBuffer DAQmxBaseCfgOutputBuffer
51 #define MRPT_DAQmxStartTask DAQmxBaseStartTask
52 #define MRPT_DAQmxStopTask DAQmxBaseStopTask
53 #define MRPT_DAQmxClearTask DAQmxBaseClearTask
54 #define MRPT_DAQmxReadAnalogF64 DAQmxBaseReadAnalogF64
55 #define MRPT_DAQmxReadCounterF64 DAQmxBaseReadCounterF64
56 #define MRPT_DAQmxReadDigitalU8 DAQmxBaseReadDigitalU8
57 #define MRPT_DAQmxWriteAnalogF64 DAQmxBaseWriteAnalogF64
58 #define MRPT_DAQmxWriteDigitalU32 DAQmxBaseWriteDigitalU32
59 #define MRPT_DAQmxWriteDigitalLines DAQmxBaseWriteDigitalLines
60 #else
61 #define MRPT_DAQmxGetExtendedErrorInfo DAQmxGetExtendedErrorInfo
62 #define MRPT_DAQmxCreateTask DAQmxCreateTask
63 #define MRPT_DAQmxCreateAIVoltageChan DAQmxCreateAIVoltageChan
64 #define MRPT_DAQmxCreateAOVoltageChan DAQmxCreateAOVoltageChan
65 #define MRPT_DAQmxCreateDIChan DAQmxCreateDIChan
66 #define MRPT_DAQmxCreateDOChan DAQmxCreateDOChan
67 #define MRPT_DAQmxCreateCIPeriodChan DAQmxCreateCIPeriodChan
68 #define MRPT_DAQmxCreateCICountEdgesChan DAQmxCreateCICountEdgesChan
69 #define MRPT_DAQmxCreateCIPulseWidthChan DAQmxCreateCIPulseWidthChan
70 #define MRPT_DAQmxCreateCILinEncoderChan DAQmxCreateCILinEncoderChan
71 #define MRPT_DAQmxCreateCIAngEncoderChan DAQmxCreateCIAngEncoderChan
72 #define MRPT_DAQmxCreateCOPulseChanFreq DAQmxCreateCOPulseChanFreq
73 #define MRPT_DAQmxCfgSampClkTiming DAQmxCfgSampClkTiming
74 #define MRPT_DAQmxCfgInputBuffer DAQmxCfgInputBuffer
75 #define MRPT_DAQmxCfgOutputBuffer DAQmxCfgOutputBuffer
76 #define MRPT_DAQmxStartTask DAQmxStartTask
77 #define MRPT_DAQmxStopTask DAQmxStopTask
78 #define MRPT_DAQmxClearTask DAQmxClearTask
79 #define MRPT_DAQmxReadAnalogF64 DAQmxReadAnalogF64
80 #define MRPT_DAQmxReadCounterF64 DAQmxReadCounterF64
81 #define MRPT_DAQmxReadDigitalU8 DAQmxReadDigitalU8
82 #define MRPT_DAQmxWriteAnalogF64 DAQmxWriteAnalogF64
83 #define MRPT_DAQmxWriteDigitalU32 DAQmxWriteDigitalU32
84 #define MRPT_DAQmxWriteDigitalLines DAQmxWriteDigitalLines
85 #endif
86
87 // An auxiliary macro to check and report errors in the DAQmx library as
88 // exceptions with a well-explained message.
89 #define MRPT_DAQmx_ErrChk(functionCall) \
90 if ((functionCall) < 0) \
91 { \
92 char errBuff[2048]; \
93 MRPT_DAQmxGetExtendedErrorInfo(errBuff, 2048); \
94 std::string sErr = mrpt::format("DAQ error: '%s'\nCalling: '%s'", errBuff, #functionCall); \
95 THROW_EXCEPTION(sErr); \
96 }
97
98 using namespace mrpt::hwdrivers;
99 using namespace mrpt::obs;
100 using namespace mrpt::system;
101 using namespace std;
102
103 IMPLEMENTS_GENERIC_SENSOR(CNationalInstrumentsDAQ, mrpt::hwdrivers)
104
105 // ------------- CNationalInstrumentsDAQ::TInfoPerTask -----------
106 // Default ctor:
107 CNationalInstrumentsDAQ::TInfoPerTask::TInfoPerTask() : new_obs_available(0), task() {}
108
109 /* -----------------------------------------------------
110 Constructor
111 ----------------------------------------------------- */
112 CNationalInstrumentsDAQ::CNationalInstrumentsDAQ() :
113 mrpt::system::COutputLogger("CNationalInstrumentsDAQ")
114 {
115 m_sensorLabel = "NIDAQ";
116 }
117
118 // Just like "MRPT_LOAD_HERE_CONFIG_VAR" but...
119 #define MY_LOAD_HERE_CONFIG_VAR( \
120 variableName, variableType, targetVariable, configFileObject, sectionNameStr) \
121 targetVariable = \
122 configFileObject.read_##variableType(sectionNameStr, variableName, targetVariable, false);
123
124 #define MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT( \
125 variableName, variableType, targetVariable, configFileObject, sectionNameStr) \
126 { \
127 try \
128 { \
129 targetVariable = configFileObject.read_##variableType( \
130 sectionNameStr, variableName, targetVariable, true); \
131 } \
132 catch (std::exception&) \
133 { \
134 THROW_EXCEPTION(mrpt::format( \
135 "Value for '%s' not found in config file", std::string(variableName).c_str())); \
136 } \
137 }
138
139 /* -----------------------------------------------------
140 loadConfig_sensorSpecific
141 ----------------------------------------------------- */
142 void CNationalInstrumentsDAQ::loadConfig_sensorSpecific(
143 const mrpt::config::CConfigFileBase& cfg, const std::string& sect)
144 {
145 // std::vector<TaskDescription> task_definitions;
146 task_definitions.clear();
147
148 const unsigned int nTasks = cfg.read_uint64_t(sect, "num_tasks", 0, true);
149 if (!nTasks)
150 {
151 std::cerr << "[CNationalInstrumentsDAQ] Warning: Number of tasks is "
152 "zero. No datalogging will be done.\n";
153 }
154
155 task_definitions.resize(nTasks);
156 for (unsigned int i = 0; i < nTasks; i++)
157 {
158 TaskDescription& t = task_definitions[i];
159 const string sTask = mrpt::format("task%u", i);
160
161 // Read general settings for this task:
162 // ---------------------------------------
163 const string sChanns = cfg.read_string(sect, sTask + string(".channels"), "", true);
164 vector<string> lstStrChanns;
165 mrpt::system::tokenize(sChanns, " \t,", lstStrChanns);
166 if (lstStrChanns.empty()) THROW_EXCEPTION_FMT("List of channels for task %u is empty!", i);
167
168 MY_LOAD_HERE_CONFIG_VAR(
169 sTask + string(".samplesPerSecond"), double, t.samplesPerSecond, cfg, sect)
170 MY_LOAD_HERE_CONFIG_VAR(
171 sTask + string(".samplesPerChannelToRead"), double, t.samplesPerChannelToRead, cfg, sect)
172 MY_LOAD_HERE_CONFIG_VAR(
173 sTask + string(".sampleClkSource"), string, t.sampleClkSource, cfg, sect)
174 MY_LOAD_HERE_CONFIG_VAR(
175 sTask + string(".bufferSamplesPerChannel"), double, t.bufferSamplesPerChannel, cfg, sect)
176 t.taskLabel = cfg.read_string(sect, sTask + string(".taskLabel"), sTask, false);
177
178 for (auto& lstStrChann : lstStrChanns)
179 {
180 if (strCmpI(lstStrChann, "ai"))
181 {
182 t.has_ai = true;
183 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
184 sTask + string(".ai.physicalChannel"), string, t.ai.physicalChannel, cfg, sect)
185 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
186 sTask + string(".ai.physicalChannelCount"), uint64_t, t.ai.physicalChannelCount, cfg,
187 sect)
188 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
189 sTask + string(".ai.terminalConfig"), string, t.ai.terminalConfig, cfg, sect)
190 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
191 sTask + string(".ai.minVal"), double, t.ai.minVal, cfg, sect)
192 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
193 sTask + string(".ai.maxVal"), double, t.ai.maxVal, cfg, sect)
194 }
195 else if (strCmpI(lstStrChann, "ao"))
196 {
197 t.has_ao = true;
198 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
199 sTask + string(".ao.physicalChannel"), string, t.ao.physicalChannel, cfg, sect)
200 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
201 sTask + string(".ao.physicalChannelCount"), uint64_t, t.ao.physicalChannelCount, cfg,
202 sect)
203 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
204 sTask + string(".ao.minVal"), double, t.ao.minVal, cfg, sect)
205 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
206 sTask + string(".ao.maxVal"), double, t.ao.maxVal, cfg, sect)
207 }
208 else if (strCmpI(lstStrChann, "di"))
209 {
210 t.has_di = true;
211 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(sTask + string(".di.line"), string, t.di.line, cfg, sect)
212 }
213 else if (strCmpI(lstStrChann, "do"))
214 {
215 t.has_do = true;
216 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
217 sTask + string(".do.line"), string, t.douts.line, cfg, sect)
218 }
219 else if (strCmpI(lstStrChann, "ci_period"))
220 {
221 t.has_ci_period = true;
222 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
223 sTask + string(".ci_period.counter"), string, t.ci_period.counter, cfg, sect)
224 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
225 sTask + string(".ci_period.minVal"), double, t.ci_period.minVal, cfg, sect)
226 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
227 sTask + string(".ci_period.maxVal"), double, t.ci_period.maxVal, cfg, sect)
228 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
229 sTask + string(".ci_period.units"), string, t.ci_period.units, cfg, sect)
230 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
231 sTask + string(".ci_period.edge"), string, t.ci_period.edge, cfg, sect)
232 MY_LOAD_HERE_CONFIG_VAR(
233 sTask + string(".ci_period.measTime"), double, t.ci_period.measTime, cfg, sect)
234 MY_LOAD_HERE_CONFIG_VAR(
235 sTask + string(".ci_period.divisor"), int, t.ci_period.divisor, cfg, sect)
236 }
237 else if (strCmpI(lstStrChann, "ci_count_edges"))
238 {
239 t.has_ci_count_edges = true;
240 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
241 sTask + string(".ci_count_edges.counter"), string, t.ci_count_edges.counter, cfg, sect)
242 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
243 sTask + string(".ci_count_edges.edge"), string, t.ci_count_edges.edge, cfg, sect)
244 MY_LOAD_HERE_CONFIG_VAR(
245 sTask + string(".ci_count_edges.initialCount"), int, t.ci_count_edges.initialCount, cfg,
246 sect)
247 MY_LOAD_HERE_CONFIG_VAR(
248 sTask + string(".ci_count_edges.countDirection"), string,
249 t.ci_count_edges.countDirection, cfg, sect)
250 }
251 else if (strCmpI(lstStrChann, "ci_pulse_width"))
252 {
253 t.has_ci_pulse_width = true;
254 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
255 sTask + string(".ci_pulse_width.counter"), string, t.ci_pulse_width.counter, cfg, sect)
256 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
257 sTask + string(".ci_pulse_width.minVal"), double, t.ci_pulse_width.minVal, cfg, sect)
258 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
259 sTask + string(".ci_pulse_width.maxVal"), double, t.ci_pulse_width.maxVal, cfg, sect)
260 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
261 sTask + string(".ci_pulse_width.units"), string, t.ci_pulse_width.units, cfg, sect)
262 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
263 sTask + string(".ci_pulse_width.startingEdge"), string, t.ci_pulse_width.startingEdge,
264 cfg, sect)
265 }
266 else if (strCmpI(lstStrChann, "ci_lin_encoder"))
267 {
268 t.has_ci_lin_encoder = true;
269 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
270 sTask + string(".ci_lin_encoder.counter"), string, t.ci_lin_encoder.counter, cfg, sect)
271 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
272 sTask + string(".ci_lin_encoder.decodingType"), string, t.ci_lin_encoder.decodingType,
273 cfg, sect)
274 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
275 sTask + string(".ci_lin_encoder.ZidxEnable"), bool, t.ci_lin_encoder.ZidxEnable, cfg,
276 sect)
277 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
278 sTask + string(".ci_lin_encoder.ZidxVal"), double, t.ci_lin_encoder.ZidxVal, cfg, sect)
279 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
280 sTask + string(".ci_lin_encoder.ZidxPhase"), string, t.ci_lin_encoder.ZidxPhase, cfg,
281 sect)
282 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
283 sTask + string(".ci_lin_encoder.units"), string, t.ci_lin_encoder.units, cfg, sect)
284 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
285 sTask + string(".ci_lin_encoder.distPerPulse"), double, t.ci_lin_encoder.distPerPulse,
286 cfg, sect)
287 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
288 sTask + string(".ci_lin_encoder.initialPos"), double, t.ci_lin_encoder.initialPos, cfg,
289 sect)
290 }
291 else if (strCmpI(lstStrChann, "ci_ang_encoder"))
292 {
293 t.has_ci_ang_encoder = true;
294 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
295 sTask + string(".ci_ang_encoder.counter"), string, t.ci_ang_encoder.counter, cfg, sect)
296 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
297 sTask + string(".ci_ang_encoder.decodingType"), string, t.ci_ang_encoder.decodingType,
298 cfg, sect)
299 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
300 sTask + string(".ci_ang_encoder.ZidxEnable"), bool, t.ci_ang_encoder.ZidxEnable, cfg,
301 sect)
302 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
303 sTask + string(".ci_ang_encoder.ZidxVal"), double, t.ci_ang_encoder.ZidxVal, cfg, sect)
304 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
305 sTask + string(".ci_ang_encoder.ZidxPhase"), string, t.ci_ang_encoder.ZidxPhase, cfg,
306 sect)
307 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
308 sTask + string(".ci_ang_encoder.units"), string, t.ci_ang_encoder.units, cfg, sect)
309 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
310 sTask + string(".ci_ang_encoder.pulsesPerRev"), int, t.ci_ang_encoder.pulsesPerRev, cfg,
311 sect)
312 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
313 sTask + string(".ci_ang_encoder.initialAngle"), double, t.ci_ang_encoder.initialAngle,
314 cfg, sect)
315 MY_LOAD_HERE_CONFIG_VAR(
316 sTask + string(".ci_ang_encoder.decimate"), int, t.ci_ang_encoder.decimate, cfg, sect)
317 }
318 else if (strCmpI(lstStrChann, "co_pulses"))
319 {
320 t.has_co_pulses = true;
321 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
322 sTask + string(".co_pulses.counter"), string, t.co_pulses.counter, cfg, sect)
323 MY_LOAD_HERE_CONFIG_VAR(
324 sTask + string(".co_pulses.idleState"), string, t.co_pulses.idleState, cfg, sect)
325 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
326 sTask + string(".co_pulses.initialDelay"), double, t.co_pulses.initialDelay, cfg, sect)
327 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
328 sTask + string(".co_pulses.freq"), double, t.co_pulses.freq, cfg, sect)
329 MY_LOAD_HERE_CONFIG_VAR_NO_DEFAULT(
330 sTask + string(".co_pulses.dutyCycle"), double, t.co_pulses.dutyCycle, cfg, sect)
331 }
332 else
333 {
334 THROW_EXCEPTION_FMT(
335 "Unknown channel type '%s'! See the docs of "
336 "CNationalInstrumentsDAQ",
337 lstStrChann.c_str());
338 }
339 } // end for each "k" channel in channel "i"
340 } // end for "i", each task
341 }
342
343 /* -----------------------------------------------------
344 Destructor
345 ----------------------------------------------------- */
346 CNationalInstrumentsDAQ::~CNationalInstrumentsDAQ() { this->stop(); }
347 #if MRPT_HAS_SOME_NIDAQMX
348 // Declare a table to convert strings to their DAQmx #define values:
349 struct daqmx_str_val
350 {
351 const char* str;
352 int val;
353 };
354
355 const daqmx_str_val daqmx_vals[] = {
356 { "DAQmx_Val_Cfg_Default", DAQmx_Val_Cfg_Default},
357 { "DAQmx_Val_RSE", DAQmx_Val_RSE},
358 { "DAQmx_Val_NRSE", DAQmx_Val_NRSE},
359 { "DAQmx_Val_Diff", DAQmx_Val_Diff},
360 { "DAQmx_Val_Seconds", DAQmx_Val_Seconds},
361 { "DAQmx_Val_Rising", DAQmx_Val_Rising},
362 { "DAQmx_Val_Falling", DAQmx_Val_Falling},
363 { "DAQmx_Val_CountUp", DAQmx_Val_CountUp},
364 { "DAQmx_Val_CountDown", DAQmx_Val_CountDown},
365 {"DAQmx_Val_ExtControlled", DAQmx_Val_ExtControlled},
366 { "DAQmx_Val_AHighBHigh", DAQmx_Val_AHighBHigh},
367 { "DAQmx_Val_AHighBLow", DAQmx_Val_AHighBLow},
368 { "DAQmx_Val_ALowBHigh", DAQmx_Val_ALowBHigh},
369 { "DAQmx_Val_ALowBLow", DAQmx_Val_ALowBLow},
370 { "DAQmx_Val_X1", DAQmx_Val_X1},
371 { "DAQmx_Val_X2", DAQmx_Val_X2},
372 { "DAQmx_Val_X4", DAQmx_Val_X4},
373 { "DAQmx_Val_Meters", DAQmx_Val_Meters},
374 { "DAQmx_Val_Inches", DAQmx_Val_Inches},
375 { "DAQmx_Val_Ticks", DAQmx_Val_Ticks},
376 { "DAQmx_Val_Degrees", DAQmx_Val_Degrees},
377 { "DAQmx_Val_Radians", DAQmx_Val_Radians},
378 { "DAQmx_Val_High", DAQmx_Val_High},
379 { "DAQmx_Val_Low", DAQmx_Val_Low}
380 };
381
382 int daqmx_defstr2num(const std::string& str)
383 {
384 const std::string s = mrpt::system::trim(str);
385
386 for (unsigned int i = 0; i < sizeof(daqmx_vals) / sizeof(daqmx_vals[0]); i++)
387 {
388 if (strCmpI(daqmx_vals[i].str, s.c_str())) return daqmx_vals[i].val;
389 }
390 THROW_EXCEPTION_FMT("Error: Unknown DAQmx constant: %s", s.c_str());
391 }
392 #endif
393
394 /* -----------------------------------------------------
395 initialize
396 ----------------------------------------------------- */
397 void CNationalInstrumentsDAQ::initialize()
398 {
399 #if MRPT_HAS_SOME_NIDAQMX
400 this->stop();
401
402 for (size_t i = 0; i < task_definitions.size(); i++)
403 {
404 const TaskDescription& tf = task_definitions[i];
405
406 // Try to create a new task:
407 m_running_tasks.push_back(TInfoPerTask());
408 TInfoPerTask& ipt = m_running_tasks.back();
409 ipt.task = tf; // Save a copy of the task info for the thread to have
410 // all the needed info
411
412 try
413 {
414 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&ipt.taskHandle);
415
416 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateTask("", &taskHandle));
417
418 if (tf.has_ai)
419 {
420 ASSERTMSG_(
421 tf.ai.physicalChannelCount > 0,
422 "ai.physicalChannelCount is zero! Please, define it "
423 "correctly.")
424
425 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateAIVoltageChan(
426 taskHandle, tf.ai.physicalChannel.c_str(), nullptr,
427 daqmx_defstr2num(tf.ai.terminalConfig), tf.ai.minVal, tf.ai.maxVal, DAQmx_Val_Volts,
428 nullptr));
429 }
430 if (tf.has_ao)
431 {
432 ASSERTMSG_(
433 tf.ao.physicalChannelCount > 0,
434 "ai.physicalChannelCount is zero! Please, define it "
435 "correctly.")
436
437 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateAOVoltageChan(
438 taskHandle, tf.ao.physicalChannel.c_str(), nullptr, tf.ao.minVal, tf.ao.maxVal,
439 DAQmx_Val_Volts, nullptr));
440 }
441 if (tf.has_di)
442 {
443 MRPT_DAQmx_ErrChk(
444 MRPT_DAQmxCreateDIChan(taskHandle, tf.di.line.c_str(), nullptr, DAQmx_Val_ChanPerLine));
445 }
446 if (tf.has_do)
447 {
448 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateDOChan(
449 taskHandle, tf.douts.line.c_str(), nullptr, DAQmx_Val_ChanPerLine));
450 }
451 if (tf.has_ci_period)
452 {
453 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCIPeriodChan(
454 taskHandle, tf.ci_period.counter.c_str(), nullptr, tf.ci_period.minVal,
455 tf.ci_period.maxVal, daqmx_defstr2num(tf.ci_period.units),
456 daqmx_defstr2num(tf.ci_period.edge), DAQmx_Val_LowFreq1Ctr, tf.ci_period.measTime,
457 tf.ci_period.divisor, nullptr));
458 }
459 if (tf.has_ci_count_edges)
460 {
461 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCICountEdgesChan(
462 taskHandle, tf.ci_count_edges.counter.c_str(), nullptr,
463 daqmx_defstr2num(tf.ci_count_edges.edge), tf.ci_count_edges.initialCount,
464 daqmx_defstr2num(tf.ci_count_edges.countDirection)));
465 }
466 if (tf.has_ci_pulse_width)
467 {
468 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCIPulseWidthChan(
469 taskHandle, tf.ci_pulse_width.counter.c_str(), nullptr, tf.ci_pulse_width.minVal,
470 tf.ci_pulse_width.maxVal, daqmx_defstr2num(tf.ci_pulse_width.units),
471 daqmx_defstr2num(tf.ci_pulse_width.startingEdge), nullptr));
472 }
473 if (tf.has_ci_lin_encoder)
474 {
475 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCILinEncoderChan(
476 taskHandle, tf.ci_lin_encoder.counter.c_str(), nullptr,
477 daqmx_defstr2num(tf.ci_lin_encoder.decodingType), tf.ci_lin_encoder.ZidxEnable,
478 tf.ci_lin_encoder.ZidxVal, daqmx_defstr2num(tf.ci_lin_encoder.ZidxPhase),
479 daqmx_defstr2num(tf.ci_lin_encoder.units), tf.ci_lin_encoder.distPerPulse,
480 tf.ci_lin_encoder.initialPos, nullptr));
481 }
482 if (tf.has_ci_ang_encoder)
483 {
484 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCIAngEncoderChan(
485 taskHandle, tf.ci_ang_encoder.counter.c_str(), nullptr,
486 daqmx_defstr2num(tf.ci_ang_encoder.decodingType), tf.ci_ang_encoder.ZidxEnable,
487 tf.ci_ang_encoder.ZidxVal, daqmx_defstr2num(tf.ci_ang_encoder.ZidxPhase),
488 daqmx_defstr2num(tf.ci_ang_encoder.units), tf.ci_ang_encoder.pulsesPerRev,
489 tf.ci_ang_encoder.initialAngle, nullptr));
490 }
491 if (tf.has_co_pulses)
492 {
493 MRPT_DAQmx_ErrChk(MRPT_DAQmxCreateCOPulseChanFreq(
494 taskHandle, tf.co_pulses.counter.c_str(), nullptr, DAQmx_Val_Hz,
495 daqmx_defstr2num(tf.co_pulses.idleState), tf.co_pulses.initialDelay, tf.co_pulses.freq,
496 tf.co_pulses.dutyCycle));
497 }
498
499 // Seems to be needed to avoid an errors avoid like:
500 // " Onboard device memory overflow. Because of system and/or
501 // bus-bandwidth limitations, the driver could not read data from
502 // the device fast enough to keep up with the device throughput."
503 if (tf.has_ai || tf.has_di || tf.has_ci_period || tf.has_ci_count_edges ||
504 tf.has_ci_pulse_width || tf.has_ci_lin_encoder || tf.has_ci_ang_encoder)
505 {
506 // sample rate:
507 ASSERT_GT_(tf.samplesPerSecond, 0)
508 MRPT_DAQmx_ErrChk(MRPT_DAQmxCfgSampClkTiming(
509 taskHandle, tf.sampleClkSource.c_str(), tf.samplesPerSecond, DAQmx_Val_Rising,
510 DAQmx_Val_ContSamps, tf.samplesPerChannelToRead));
511
512 MRPT_DAQmx_ErrChk(MRPT_DAQmxCfgInputBuffer(taskHandle, tf.bufferSamplesPerChannel));
513 }
514
515 if (tf.has_ao)
516 {
517 // Nothing to do as long as we only need "on demand" outputs.
518 // MRPT_DAQmx_ErrChk (MRPT_DAQmxCfgOutputBuffer(taskHandle,2
519 ///*tf.bufferSamplesPerChannel*/ ));
520 // // Output buffer MUST have some data before starting the
521 // task: write 0s:
522 // vector<double> d;
523 // d.assign(tf.ao.physicalChannelCount*2, 0.0);
524 // this->writeAnalogOutputTask(i,1 /* samples per channel */,
525 //&d[0], 0.10 /*timeout*/, false);
526 }
527
528 // Create pipe:
529 mrpt::synch::CPipe::createPipe(ipt.read_pipe, ipt.write_pipe);
530
531 // Add a large timeout, just in case the writing thread dies
532 // unexpectedly so the reader doesn't hang on:
533 ipt.read_pipe->timeout_read_start_us = 100000; // 100ms
534 ipt.read_pipe->timeout_read_between_us = 100000; // 100ms
535
536 MRPT_DAQmx_ErrChk(MRPT_DAQmxStartTask(taskHandle));
537
538 ipt.hThread = std::thread(&CNationalInstrumentsDAQ::grabbing_thread, this, ipt);
539 }
540 catch (std::exception const& e)
541 {
542 std::cerr << "[CNationalInstrumentsDAQ] Error:" << std::endl << e.what() << std::endl;
543 if (ipt.taskHandle != nullptr)
544 {
545 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&ipt.taskHandle);
546 MRPT_DAQmxStopTask(taskHandle);
547 MRPT_DAQmxClearTask(taskHandle);
548 }
549
550 // Stop thread:
551 if (ipt.hThread.joinable())
552 {
553 ipt.must_close = true;
554 cerr << "[CNationalInstrumentsDAQ::initialize] Waiting for the "
555 "grabbing thread to end due to exception...\n";
556 ipt.hThread.join();
557 cerr << "[CNationalInstrumentsDAQ::initialize] Grabbing thread "
558 "ended.\n";
559 }
560
561 // Remove from list:
562 m_running_tasks.erase(--m_running_tasks.end());
563
564 std::cerr << "[CNationalInstrumentsDAQ] Error while creating "
565 "tasks. Closing other tasks before returning...\n";
566 this->stop();
567 std::cerr << "[CNationalInstrumentsDAQ] Closing tasks done.\n";
568
569 throw; // Rethrow
570 }
571 } // end for each task_definitions[i]
572
573 #else
574 THROW_EXCEPTION("MRPT was compiled without support for NI DAQmx!!");
575 #endif
576 }
577
578 /** Stop the grabbing threads for DAQ tasks. It is automatically called at
579 * destruction. */
580 void CNationalInstrumentsDAQ::stop()
581 {
582 // Stop all threads:
583 for (auto& m_running_task : m_running_tasks)
584 {
585 m_running_task.must_close = true;
586 }
587 if (m_verbose)
588 cout << "[CNationalInstrumentsDAQ::stop] Waiting for grabbing threads "
589 "to end...\n";
590 for (auto& m_running_task : m_running_tasks)
591 {
592 // For some reason, join doesn't work...
593 if (m_running_task.hThread.joinable()) m_running_task.hThread.join();
594 // Polling:
595 // for (size_t tim=0;tim<250 && !it->is_closed;tim++) {
596 // std::this_thread::sleep_for(1ms); }
597 // it->hThread.clear();
598 }
599 if (m_verbose) cout << "[CNationalInstrumentsDAQ::stop] All threads ended.\n";
600
601 // Stop all NI tasks:
602 #if MRPT_HAS_SOME_NIDAQMX
603 for (list<TInfoPerTask>::iterator it = m_running_tasks.begin(); it != m_running_tasks.end(); ++it)
604 {
605 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&it->taskHandle);
606
607 MRPT_DAQmxStopTask(taskHandle);
608 MRPT_DAQmxClearTask(taskHandle);
609 taskHandle = nullptr;
610 }
611 #endif
612 }
613
614 /** Returns true if initialize() was called successfully. */
615 bool CNationalInstrumentsDAQ::checkDAQIsWorking() const
616 {
617 return (!m_running_tasks.empty() && !m_running_tasks.begin()->is_closed);
618 }
619
620 /*-------------------------------------------------------------
621 readFromDAQ
622 -------------------------------------------------------------*/
623 void CNationalInstrumentsDAQ::readFromDAQ(
624 std::vector<mrpt::obs::CObservationRawDAQ::Ptr>& outObservations, bool& hardwareError)
625 {
626 hardwareError = false;
627 outObservations.clear();
628
629 if (!checkDAQIsWorking())
630 {
631 hardwareError = true;
632 return;
633 }
634
635 // Read from the pipe:
636 m_state = ssWorking;
637
638 for (auto& m_running_task : m_running_tasks)
639 {
640 CObservationRawDAQ tmp_obs;
641 try
642 {
643 if (m_running_task.new_obs_available != 0)
644 {
645 auto arch = mrpt::serialization::archiveFrom(*m_running_task.read_pipe);
646 arch.ReadObject(&tmp_obs);
647 --(m_running_task.new_obs_available);
648
649 // Yes, valid block of samples was adquired:
650 outObservations.push_back(CObservationRawDAQ::Create(tmp_obs));
651 }
652 }
653 catch (...)
654 {
655 // Timeout...
656 }
657 }
658 }
659
660 /* -----------------------------------------------------
661 doProcess
662 ----------------------------------------------------- */
663 void CNationalInstrumentsDAQ::doProcess()
664 {
665 bool hwError;
666 readFromDAQ(m_nextObservations, hwError);
667
668 if (hwError)
669 {
670 m_state = ssError;
671 THROW_EXCEPTION("Couldn't start DAQ task!");
672 }
673
674 if (!m_nextObservations.empty())
675 {
676 m_state = ssWorking;
677
678 std::vector<mrpt::serialization::CSerializable::Ptr> new_obs;
679 new_obs.resize(m_nextObservations.size());
680
681 for (size_t i = 0; i < m_nextObservations.size(); i++) new_obs[i] = m_nextObservations[i];
682
683 appendObservations(new_obs);
684 }
685 }
686
687 /* -----------------------------------------------------
688 grabbing_thread
689 ----------------------------------------------------- */
690 void CNationalInstrumentsDAQ::grabbing_thread(TInfoPerTask& ipt)
691 {
692 #if MRPT_HAS_SOME_NIDAQMX
693 try
694 {
695 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&ipt.taskHandle);
696 if (m_verbose)
697 cout << "[CNationalInstrumentsDAQ::grabbing_thread] Starting "
698 "thread for task "
699 << ipt.taskHandle << "\n";
700
701 MRPT_TODO("Add write timeout")
702 // ipt.write_pipe->timeout_read_between_us
703
704 const float timeout = 10 * ipt.task.samplesPerChannelToRead / ipt.task.samplesPerSecond;
705
706 int err = 0;
707 vector<double> dBuf;
708 vector<uint8_t> u8Buf;
709
710 const mrpt::obs::CObservationRawDAQ clean_obs;
711 mrpt::obs::CObservationRawDAQ obs;
712
713 while (!ipt.must_close)
714 {
715 obs = clean_obs; // Start with an empty observation
716
717 // Common stuff:
718 obs.timestamp = mrpt::Clock::now();
719 obs.sample_rate = ipt.task.samplesPerSecond;
720 obs.sensorLabel = m_sensorLabel + string(".") + ipt.task.taskLabel;
721
722 bool there_are_data = false; // At least one channel?
723
724 // Read from each channel in this task:
725 // -----------------------------------------------
726 if (ipt.task.has_ai)
727 {
728 obs.AIN_channel_count = ipt.task.ai.physicalChannelCount;
729 obs.AIN_interleaved = true;
730
731 const uint32_t totalSamplesToRead =
732 ipt.task.ai.physicalChannelCount * ipt.task.samplesPerChannelToRead;
733 dBuf.resize(totalSamplesToRead);
734 int32 pointsReadPerChan = -1;
735 if ((err = MRPT_DAQmxReadAnalogF64(
736 taskHandle, ipt.task.samplesPerChannelToRead, timeout,
737 obs.AIN_interleaved ? DAQmx_Val_GroupByScanNumber : DAQmx_Val_GroupByChannel,
738 &dBuf[0], dBuf.size(), &pointsReadPerChan, nullptr)) < 0 &&
739 err != DAQmxErrorSamplesNotYetAvailable)
740 {
741 MRPT_DAQmx_ErrChk(err)
742 }
743 else if (pointsReadPerChan > 0)
744 {
745 ASSERT_EQUAL_(totalSamplesToRead, pointsReadPerChan * ipt.task.ai.physicalChannelCount)
746 obs.AIN_double = dBuf;
747 there_are_data = true;
748 if (m_verbose)
749 cout << "[CNationalInstrumentsDAQ::grabbing_thread] " << pointsReadPerChan
750 << " analog samples read.\n";
751 }
752 } // end AI
753 if (ipt.task.has_di)
754 {
755 const uint32_t totalSamplesToRead = 1 * ipt.task.samplesPerChannelToRead;
756 u8Buf.resize(totalSamplesToRead);
757
758 int32 pointsReadPerChan = -1;
759 if ((err = MRPT_DAQmxReadDigitalU8(
760 taskHandle, ipt.task.samplesPerChannelToRead, timeout, DAQmx_Val_GroupByChannel,
761 &u8Buf[0], u8Buf.size(), &pointsReadPerChan, nullptr)) < 0 &&
762 err != DAQmxErrorSamplesNotYetAvailable)
763 {
764 MRPT_DAQmx_ErrChk(err)
765 }
766 else if (pointsReadPerChan > 0)
767 {
768 ASSERT_EQUAL_(totalSamplesToRead, pointsReadPerChan * ipt.task.ai.physicalChannelCount)
769 obs.DIN = u8Buf;
770 there_are_data = true;
771 if (m_verbose)
772 cout << "[CNationalInstrumentsDAQ::grabbing_thread] " << pointsReadPerChan
773 << " digital samples read.\n";
774 }
775 } // end DI
776 if (ipt.task.has_ci_ang_encoder || ipt.task.has_ci_lin_encoder)
777 {
778 const int32 totalSamplesToRead = ipt.task.samplesPerChannelToRead;
779 dBuf.resize(totalSamplesToRead);
780 int32 pointsReadPerChan = -1;
781 if ((err = MRPT_DAQmxReadCounterF64(
782 taskHandle, totalSamplesToRead, timeout, &dBuf[0], dBuf.size(), &pointsReadPerChan,
783 nullptr)) < 0 &&
784 err != DAQmxErrorSamplesNotYetAvailable)
785 {
786 MRPT_DAQmx_ErrChk(err)
787 }
788 else if (pointsReadPerChan > 0)
789 {
790 ASSERT_EQUAL_(totalSamplesToRead, pointsReadPerChan);
791 // Decimate?
792 if (++ipt.task.ci_ang_encoder.decimate_cnt >= ipt.task.ci_ang_encoder.decimate)
793 {
794 ipt.task.ci_ang_encoder.decimate_cnt = 0;
795
796 obs.CNTRIN_double = dBuf;
797 there_are_data = true;
798 if (m_verbose && !obs.CNTRIN_double.empty())
799 {
800 static int decim = 0;
801 if (!decim)
802 cout << "[CNationalInstrumentsDAQ::grabbing_"
803 "thread] "
804 << pointsReadPerChan << " counter samples read ([0]=" << obs.CNTRIN_double[0]
805 << ").\n";
806 if (++decim > 100) decim = 0;
807 }
808 }
809 }
810 } // end COUNTERS
811
812 // Send the observation to the main thread:
813 if (there_are_data)
814 {
815 ++(ipt.new_obs_available);
816 ipt.write_pipe->WriteObject(&obs);
817 // std::this_thread::sleep_for(1ms); // This seems to be needed
818 // to allow all objs to be sent to the recv thread
819 }
820 else
821 {
822 std::this_thread::sleep_for(1ms);
823 }
824
825 } // end of main thread loop
826 }
827 catch (const std::exception& e)
828 {
829 std::cerr << "[CNationalInstrumentsDAQ::grabbing_thread] Exception:\n" << e.what() << std::endl;
830 }
831 #endif // MRPT_HAS_SOME_NIDAQMX
832
833 ipt.is_closed = true;
834 }
835
836 void CNationalInstrumentsDAQ::writeAnalogOutputTask(
837 [[maybe_unused]] size_t task_index,
838 [[maybe_unused]] size_t nSamplesPerChannel,
839 [[maybe_unused]] const double* volt_values,
840 [[maybe_unused]] double timeout,
841 [[maybe_unused]] bool groupedByChannel)
842 {
843 #if MRPT_HAS_SOME_NIDAQMX
844 ASSERT_(task_index < m_running_tasks.size());
845 std::list<TInfoPerTask>::iterator it = m_running_tasks.begin();
846 std::advance(it, task_index);
847 TInfoPerTask& ipt = *it;
848 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&ipt.taskHandle);
849
850 int32 samplesWritten = 0;
851 int err = 0;
852 if (err = MRPT_DAQmxWriteAnalogF64(
853 taskHandle, nSamplesPerChannel, FALSE, timeout,
854 groupedByChannel ? DAQmx_Val_GroupByChannel : DAQmx_Val_GroupByScanNumber,
855 const_cast<float64*>(volt_values), &samplesWritten, nullptr))
856 {
857 MRPT_DAQmx_ErrChk(err)
858 }
859 #endif
860 }
861
862 void CNationalInstrumentsDAQ::writeDigitalOutputTask(
863 [[maybe_unused]] size_t task_index,
864 [[maybe_unused]] bool line_value,
865 [[maybe_unused]] double timeout)
866 {
867 #if MRPT_HAS_SOME_NIDAQMX
868 ASSERT_(task_index < m_running_tasks.size());
869 std::list<TInfoPerTask>::iterator it = m_running_tasks.begin();
870 std::advance(it, task_index);
871 TInfoPerTask& ipt = *it;
872 TaskHandle& taskHandle = *reinterpret_cast<TaskHandle*>(&ipt.taskHandle);
873
874 uInt8 dat = line_value ? 1 : 0;
875
876 int32 samplesWritten = 0;
877 int32 nSamplesPerChannel = 1;
878 int err = 0;
879 if (err = MRPT_DAQmxWriteDigitalLines(
880 taskHandle, nSamplesPerChannel, FALSE, timeout, DAQmx_Val_GroupByScanNumber, &dat,
881 &samplesWritten, nullptr))
882 {
883 MRPT_DAQmx_ErrChk(err)
884 }
885 #endif
886 }
887
888 // Ctor:
889 CNationalInstrumentsDAQ::TaskDescription::TaskDescription() = default;