| 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; |