Fawkes API Fawkes Development Version
lase_edl_aqt.cpp
1
2/***************************************************************************
3 * lase_edl_aqt.cpp - Thread that retrieves the laser data
4 *
5 * Created: Wed Oct 08 13:42:32 2008
6 * Copyright 2002 Christian Fritz
7 * 2008-2009 Tim Niemueller [www.niemueller.de]
8 *
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#include "lase_edl_aqt.h"
25
26#include <core/threading/mutex.h>
27
28#include <cmath>
29#include <cstdio>
30#include <cstdlib>
31#include <string>
32#include <vector>
33
34using namespace fawkes;
35
36const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESET = 0x0000;
37const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESTART = 0x0001;
38const WORD LaseEdlAcquisitionThread::RESETLEVEL_HALT_IDLE = 0x0002;
39const WORD LaseEdlAcquisitionThread::RESETLEVEL_RELOAD_VOLTSET = 0x0010;
40const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET_HISTORIC = 0x0000;
41const WORD LaseEdlAcquisitionThread::CONFIGITEM_RS232_RS422 = 0x0001;
42const WORD LaseEdlAcquisitionThread::CONFIGITEM_CAN = 0x0002;
43const WORD LaseEdlAcquisitionThread::CONFIGITEM_SPI = 0x0003;
44const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET = 0x0004;
45const WORD LaseEdlAcquisitionThread::CONFIGITEM_GLOBAL = 0x0010;
46const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_RS232_RS422 = 4;
47const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_CAN = 5;
48const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_ARCNET = 2;
49const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_GLOBAL = 3;
50const WORD LaseEdlAcquisitionThread::SECTOR_0 = 0x0000;
51const WORD LaseEdlAcquisitionThread::SECTOR_1 = 0x0001;
52const WORD LaseEdlAcquisitionThread::SECTOR_2 = 0x0002;
53const WORD LaseEdlAcquisitionThread::SECTOR_3 = 0x0003;
54const WORD LaseEdlAcquisitionThread::SECTOR_4 = 0x0004;
55const WORD LaseEdlAcquisitionThread::SECTOR_5 = 0x0005;
56const WORD LaseEdlAcquisitionThread::SECTOR_6 = 0x0006;
57const WORD LaseEdlAcquisitionThread::SECTOR_7 = 0x0007;
58const WORD LaseEdlAcquisitionThread::SECTORFUNC_NOT_INITIALIZED = 0x0000;
59const WORD LaseEdlAcquisitionThread::SECTORFUNC_NO_MEASUREMENT = 0x0001;
60const WORD LaseEdlAcquisitionThread::SECTORFUNC_DUMMY_MEASUREMENT = 0x0002;
61const WORD LaseEdlAcquisitionThread::SECTORFUNC_NORMAL_MEASUREMENT = 0x0003;
62const WORD LaseEdlAcquisitionThread::SECTORFUNC_REFERENCE_TARGET = 0x0004;
63const WORD LaseEdlAcquisitionThread::FLASH_YES = 0x0001;
64const WORD LaseEdlAcquisitionThread::FLASH_NO = 0x0000;
65const WORD LaseEdlAcquisitionThread::PROFILENUM_CONTINUOUS = 0x0000;
66const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUMBER = 0x0001;
67const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_COUNTER = 0x0002;
68const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_LAYER = 0x0004;
69const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SECTOR = 0x0008;
70const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ANGLE_STEP = 0x0010;
71const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUM_SECT_POINTS = 0x0020;
72const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_START = 0x0040;
73const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_START_DIRECTION = 0x0080;
74const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DISTANCE = 0x0100;
75const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DIRECTION = 0x0200;
76const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ECHO_AMPLITUDE = 0x0400;
77const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_END = 0x0800;
78const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_END_DIRECTION = 0x1000;
79const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SENSOR_MODE = 0x2000;
80
81const WORD LaseEdlAcquisitionThread::SERVICEGROUP_STATUS = 0x0100;
82const WORD LaseEdlAcquisitionThread::CMD_GET_IDENTIFICATION = 0x0101;
83const WORD LaseEdlAcquisitionThread::CMD_GET_STATUS = 0x0102;
84const WORD LaseEdlAcquisitionThread::CMD_GET_ERROR = 0x0103;
85const WORD LaseEdlAcquisitionThread::CMD_GET_SIGNAL = 0x0104;
86const WORD LaseEdlAcquisitionThread::CMD_SET_SIGNAL = 0x0105;
87const WORD LaseEdlAcquisitionThread::CMD_REGISTER_APPLICATION = 0x0106;
88const WORD LaseEdlAcquisitionThread::SERVICEGROUP_CONFIG = 0x0200;
89const WORD LaseEdlAcquisitionThread::CMD_SET_CONFIG = 0x0201;
90const WORD LaseEdlAcquisitionThread::CMD_GET_CONFIG = 0x0202;
91const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_ABS = 0x0203;
92const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_REL = 0x0204;
93const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_CLOCK = 0x0205;
94const WORD LaseEdlAcquisitionThread::CMD_SET_ZONE = 0x0206;
95const WORD LaseEdlAcquisitionThread::CMD_GET_ZONE = 0x0207;
96const WORD LaseEdlAcquisitionThread::CMD_RELEASE_ZONE = 0x0208;
97const WORD LaseEdlAcquisitionThread::CMD_SET_FILTER = 0x0209;
98const WORD LaseEdlAcquisitionThread::CMD_SET_FUNCTION = 0x020A;
99const WORD LaseEdlAcquisitionThread::CMD_GET_FUNCTION = 0x020B;
100const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MEASUREMENT = 0x0300;
101const WORD LaseEdlAcquisitionThread::CMD_GET_PROFILE = 0x0301;
102const WORD LaseEdlAcquisitionThread::CMD_CANCEL_PROFILE = 0x0302;
103const WORD LaseEdlAcquisitionThread::SERVICEGROUP_WORKING = 0x0400;
104const WORD LaseEdlAcquisitionThread::CMD_DO_RESET = 0x0401;
105const WORD LaseEdlAcquisitionThread::CMD_TRANS_IDLE = 0x0402;
106const WORD LaseEdlAcquisitionThread::CMD_TRANS_ROTATE = 0x0403;
107const WORD LaseEdlAcquisitionThread::CMD_TRANS_MEASURE = 0x0404;
108const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MAINTENANCE = 0x0500;
109const WORD LaseEdlAcquisitionThread::CMD_DO_ADJUST = 0x0501;
110const WORD LaseEdlAcquisitionThread::CMD_DO_TEST = 0x0502;
111const WORD LaseEdlAcquisitionThread::SERVICEGROUP_INTERFACE_ROUTING = 0x0600;
112const WORD LaseEdlAcquisitionThread::CMD_COM_ATTACH = 0x0601;
113const WORD LaseEdlAcquisitionThread::CMD_COM_DETACH = 0x0602;
114const WORD LaseEdlAcquisitionThread::CMD_COM_INIT = 0x0603;
115const WORD LaseEdlAcquisitionThread::CMD_COM_OUTPUT = 0x0604;
116const WORD LaseEdlAcquisitionThread::CMD_COM_DATA = 0x0605;
117const WORD LaseEdlAcquisitionThread::SERVICEGROUP_FILE = 0x0700;
118const WORD LaseEdlAcquisitionThread::CMD_DIR = 0x0701;
119const WORD LaseEdlAcquisitionThread::CMD_SAVE = 0x0702;
120const WORD LaseEdlAcquisitionThread::CMD_LOAD = 0x0703;
121const WORD LaseEdlAcquisitionThread::CMD_DELETE = 0x0704;
122const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MONITOR = 0x0900;
123const WORD LaseEdlAcquisitionThread::CMD_MONITOR_ENABLE_LOG = 0x0801;
124const WORD LaseEdlAcquisitionThread::CMD_MONITOR_DISABLE_LOG = 0x0802;
125const WORD LaseEdlAcquisitionThread::SERVICEGROUP_ADJUST = 0x7E00;
126const WORD LaseEdlAcquisitionThread::SERVICEGROUP_SPECIAL = 0x7F00;
127const WORD LaseEdlAcquisitionThread::CMD_SERVICE_FAILURE = 0x7F00;
128const WORD LaseEdlAcquisitionThread::RESPONSE_BIT = 0x8000;
129
130const float LaseEdlAcquisitionThread::DISTANCE_FACTOR = 256.00;
131
132/** @class LaseEdlAcquisitionThread "lase_edl_aqt.h"
133 * Laser acqusition thread for Lase EDL L A laser scanner.
134 * This thread fetches the data from the laser.
135 * @author Tim Niemueller
136 * @author Christian Fritz
137 */
138
139/** Constructor.
140 * @param cfg_name short name of configuration group
141 * @param cfg_prefix configuration path prefix
142 */
143LaseEdlAcquisitionThread::LaseEdlAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
144: LaserAcquisitionThread("LaseEdlAcquisitionThread")
145{
146 set_name("LaseEDL(%s)", cfg_name.c_str());
147 pre_init_done_ = false;
148 cfg_name_ = cfg_name;
149 cfg_prefix_ = cfg_prefix;
150}
151
152void
154{
155 if (pre_init_done_)
156 return;
157
158 try {
159 std::string canres = config->get_string((cfg_prefix_ + "canonical_resolution").c_str());
160 if (canres == "low") {
161 cfg_rotation_freq_ = 20;
162 cfg_angle_step_ = 16;
163 } else if (canres == "high") {
164 cfg_rotation_freq_ = 15;
165 cfg_angle_step_ = 8;
166 } else {
168 "Canonical resolution %s is invalid, must be 'low' "
169 "or 'high', trying to read raw config data");
170 throw Exception("");
171 }
173 "Using canonical resolution %s, freq: %u, angle step: %u",
174 canres.c_str(),
175 cfg_rotation_freq_,
176 cfg_angle_step_);
177 } catch (Exception &e) {
178 // exceptions thrown here will propagate
179 cfg_rotation_freq_ = config->get_uint((cfg_prefix_ + "rotation_freq").c_str());
180 cfg_angle_step_ = config->get_uint((cfg_prefix_ + "angle_step").c_str());
181 }
182
183 try {
184 cfg_use_default_ = config->get_bool((cfg_prefix_ + "use_default").c_str());
185 cfg_set_default_ = config->get_bool((cfg_prefix_ + "set_default").c_str());
186 cfg_max_pulse_freq_ = config->get_uint((cfg_prefix_ + "max_pulse_freq").c_str());
187 cfg_profile_format_ = config->get_uint((cfg_prefix_ + "profile_format").c_str());
188 cfg_can_id_ = config->get_uint((cfg_prefix_ + "can_id").c_str());
189 cfg_can_id_resp_ = config->get_uint((cfg_prefix_ + "can_id_resp").c_str());
190 cfg_sensor_id_ = config->get_uint((cfg_prefix_ + "sensor_id").c_str());
191 cfg_sensor_id_resp_ = config->get_uint((cfg_prefix_ + "sensor_id_resp").c_str());
192 cfg_btr0btr1_ = config->get_uint((cfg_prefix_ + "btr0btr1").c_str());
193 cfg_port_ = config->get_uint((cfg_prefix_ + "port").c_str());
194 cfg_irq_ = config->get_uint((cfg_prefix_ + "irq").c_str());
195 cfg_num_init_tries_ = config->get_uint((cfg_prefix_ + "num_init_tries").c_str());
196 cfg_mount_rotation_ = config->get_float((cfg_prefix_ + "mount_rotation").c_str());
197
198 min_angle_step_ = calc_angle_step(cfg_rotation_freq_, cfg_max_pulse_freq_);
199 if (cfg_angle_step_ < min_angle_step_) {
201 "Configured angle step %u less than required minimum "
202 "of %u, raising to minimum",
203 cfg_angle_step_,
204 min_angle_step_);
205 cfg_angle_step_ = min_angle_step_;
206 }
207 number_of_values_ = 16 * 360 / cfg_angle_step_;
208
209 if ((number_of_values_ != 360) && (number_of_values_ != 720)) {
210 throw Exception("At the moment only configurations with 360 or 720 "
211 "laser beams are supported, but %u requested",
212 number_of_values_);
213 }
214
215 _distances_size = _echoes_size = number_of_values_;
216
217 std::string interface_type = config->get_string((cfg_prefix_ + "interface_type").c_str());
218 if (interface_type == "usb") {
219 cfg_interface_type_ = HW_USB;
220 } else {
221 throw Exception("Unknown interface type %s", interface_type.c_str());
222 }
223
224 } catch (Exception &e) {
225 e.append("Could not read all required config values for %s", name());
226 throw;
227 }
228
229 pre_init_done_ = true;
230}
231
232void
234{
236
237 init_bus();
238
239 for (unsigned int i = 1; i <= cfg_num_init_tries_; ++i) {
240 try {
241 CANCEL_PROFILE();
242 } catch (Exception &e) {
243 // ignored, happens often
244 }
245
246 try {
247 logger->log_debug("LaseEdlAcquisitionThread", "Resetting Laser");
248 DO_RESET(RESETLEVEL_HALT_IDLE);
249
250 if (!cfg_use_default_) {
251 logger->log_debug("LaseEdlAcquisitionThread", "Setting configuration");
252 // set configuration (rotation and anglestep)
253 SET_CONFIG(CONFIGITEM_GLOBAL,
254 CONFIGDATA_LENGTH_GLOBAL,
255 cfg_sensor_id_,
256 cfg_rotation_freq_,
257 cfg_angle_step_);
258
259 // set functions (sector definition)
260 SET_FUNCTION(SECTOR_0,
261 SECTORFUNC_NORMAL_MEASUREMENT,
262 (16 * 360) - cfg_angle_step_,
263 cfg_set_default_ ? FLASH_YES : FLASH_NO);
264 SET_FUNCTION(SECTOR_1,
265 SECTORFUNC_NOT_INITIALIZED,
266 0,
267 cfg_set_default_ ? FLASH_YES : FLASH_NO);
268 }
269
270 logger->log_debug("LaseEdlAcquisitionThread", "Starting rotating");
271 TRANS_ROTATE(cfg_rotation_freq_);
272 logger->log_debug("LaseEdlAcquisitionThread", "Starting measuring");
273 TRANS_MEASURE();
274 logger->log_debug("LaseEdlAcquisitionThread", "Enable profile retrieval");
275 GET_PROFILE(PROFILENUM_CONTINUOUS, cfg_profile_format_);
276
277 break; // break for loop if initialization was successful
278 } catch (Exception &e) {
279 if (i < cfg_num_init_tries_) {
280 logger->log_warn("LaseEdlAcquisitionThread",
281 "Initialization, retrying %d more times",
282 cfg_num_init_tries_ - i);
283 logger->log_warn("LaseEdlAcquisitionThread", e);
284 } else {
285 logger->log_error("LaseEdlAcquisitionThread",
286 "Initialization failed, giving up after %u tries",
287 cfg_num_init_tries_);
288 throw;
289 }
290 }
291 }
292
293 _distances = (float *)malloc(sizeof(float) * number_of_values_);
294 _echoes = (float *)malloc(sizeof(float) * number_of_values_);
295}
296
297void
299{
300 free(_distances);
301 free(_echoes);
302 _distances = _echoes = NULL;
303
304 logger->log_debug("LaseEdlAcquisitionThread", "Resetting laser");
305 DO_RESET(RESETLEVEL_HALT_IDLE);
306}
307
308void
310{
311 process_profiles();
312}
313
314unsigned int
315LaseEdlAcquisitionThread::calc_angle_step(unsigned int rotation_freq, unsigned int max_pulse_freq)
316{
317 float tmp;
318 unsigned int rv;
319 tmp = (((float)max_pulse_freq) / 360.0) / ((float)rotation_freq);
320 tmp = ceil((1 / tmp) * 16.0);
321 rv = (unsigned int)tmp;
322
323 if (rv == 7 || rv == 11 || rv == 13 || rv == 14)
324 rv++;
325
326 return rv;
327}
328
329void
330LaseEdlAcquisitionThread::init_bus()
331{
332 FILE *f = fopen("/proc/pcan", "r");
333 if (!f) {
334 throw Exception("Cannot open /proc/pcan, PCAN driver not loaded?");
335 }
336 std::vector<std::string> config_lines;
337 std::vector<std::string> device_lines;
338 char tmp[128];
339 while (fgets(tmp, sizeof(tmp), f)) {
340 if (tmp[0] == '*') {
341 config_lines.push_back(tmp);
342 } else if (tmp[0] != '\n') {
343 device_lines.push_back(tmp);
344 }
345 }
346 fclose(f);
347
348 std::vector<std::string>::iterator l;
349 for (l = config_lines.begin(); l != config_lines.end(); ++l) {
350 // proc is found
351 std::string::size_type pos = 0;
352 while ((pos = l->find("[", pos)) != std::string::npos) {
353 pos += 1;
354 std::string::size_type pos_end = l->find("]", pos);
355 if (pos_end != std::string::npos) {
356 std::string item = l->substr(pos, pos_end - pos);
357 if (item == "net") {
358 throw Exception("PCAN driver has been compiled in netdev mode, but "
359 "chardev mode is required. Please read the plugin "
360 "documentation and recompile the PCAN driver.");
361 }
362 }
363 }
364 }
365
366 handle_ = CAN_Open(cfg_interface_type_, 0, cfg_port_, cfg_irq_);
367 if (handle_ == NULL) {
368 throw Exception("Cannot open CAN bus");
369 }
370 if (CAN_Init(handle_, cfg_btr0btr1_, CAN_INIT_TYPE_ST) != CAN_ERR_OK) {
371 throw Exception("Cannot initialize CAN bus");
372 }
373}
374
375void
376LaseEdlAcquisitionThread::send(WORD *data, int n)
377{
378 TPCANMsg msg;
379 msg.ID = cfg_can_id_;
380 msg.MSGTYPE = MSGTYPE_STANDARD;
381 msg.LEN = 0;
382
383 WORD number_of_frames = 0;
384
385 // special case for less or equal two words
386 if (n <= 2) {
387 number_of_frames = 1;
388 append_to_msg((WORD)0, &msg);
389 append_to_msg((WORD)cfg_sensor_id_, &msg);
390 if (n >= 1) {
391 append_to_msg(data[0], &msg);
392 }
393 if (n == 2) {
394 append_to_msg(data[1], &msg);
395 }
396 //printf("send (1): "); print_message(&msg);
397 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
398 throw Exception("Laser send() failed (1)");
399 }
400
401 } else { // more than 2 words
402 int sent_words = 0;
403 number_of_frames = ((n - 1) / 3) + 1;
404 if ((n - 1) % 3 != 0) {
405 ++number_of_frames;
406 }
407 append_to_msg((WORD)0xFFFF, &msg);
408 append_to_msg(number_of_frames, &msg);
409 append_to_msg((WORD)cfg_sensor_id_, &msg);
410 append_to_msg(data[sent_words++], &msg);
411 // printf("send (2): "); print_message(&msg);
412 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
413 throw Exception("Laser send() failed (2)");
414 }
415
416 for (WORD f = number_of_frames - 1; f > 1; --f) {
417 msg.LEN = 0;
418 append_to_msg(f, &msg);
419 append_to_msg(data[sent_words++], &msg);
420 append_to_msg(data[sent_words++], &msg);
421 append_to_msg(data[sent_words++], &msg);
422 // printf("send (3): "); print_message(&msg);
423 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
424 throw Exception("Laser send() failed (3)");
425 }
426 }
427 // last frame
428 msg.LEN = 0;
429 append_to_msg((WORD)0x0001, &msg);
430 for (int i = sent_words; i < n; i++) {
431 append_to_msg(data[sent_words++], &msg);
432 }
433 // printf("send (4): "); print_message(&msg);
434 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
435 throw Exception("Laser send() failed (3)");
436 }
437 }
438}
439
440int
441LaseEdlAcquisitionThread::recv(WORD **data, bool allocate)
442{
443 TPCANMsg msg;
444 // read from CAN BUS
445 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
446 throw Exception("Laser recv() failed (1)");
447 }
448 // If msg wasn't send by our laser: ignore it
449 if (msg.ID != cfg_can_id_resp_) {
450 logger->log_warn("LaseEdlAcquisitionThread",
451 "CAN ID is not the expected ID, "
452 "ignoring message");
453 return -1;
454 }
455
456 int number_of_incoming_frames = 0;
457 WORD number_of_incoming_words = 0;
458 int msg_index = 0;
459 int data_index = 0;
460 WORD read;
461
462 read = get_word_from_msg(&msg, &msg_index);
463
464 // seek for beginning of a block
465 while ((read != 0x0000) && (read != 0xFFFF)) {
466 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
467 throw Exception("Laser recv() failed (2)");
468 }
469 msg_index = 0;
470 read = get_word_from_msg(&msg, &msg_index);
471 }
472
473 // got legal block: process it
474 if (read == 0x0000) { // receiving only one frame
475 read = get_word_from_msg(&msg, &msg_index);
476 if (read != cfg_sensor_id_resp_) {
477 logger->log_warn("LaseEdlAcquisitionThread",
478 "Sensor ID is not the expected ID, "
479 "ignoring message");
480 return -1;
481 }
482 number_of_incoming_words = (msg.LEN - msg_index) / 2;
483 if (allocate) {
484 (*data) = (WORD *)malloc(sizeof(WORD) * (number_of_incoming_words));
485 }
486 for (int i = 0; i < number_of_incoming_words; ++i) {
487 (*data)[i] = get_word_from_msg(&msg, &msg_index);
488 }
489 // printf("Received (1): "); print_word_array(number_of_incoming_words, *data);
490 return number_of_incoming_words;
491 } else if (read == 0xFFFF) {
492 // get number of incoming frames
493 number_of_incoming_frames = get_word_from_msg(&msg, &msg_index);
494 if (allocate) {
495 (*data) = (WORD *)malloc(sizeof(WORD) * (number_of_incoming_frames * 6 + 1));
496 }
497 data_index = 0;
498
499 // get sensor response ID
500 read = get_word_from_msg(&msg, &msg_index);
501 if (read != cfg_sensor_id_resp_) {
502 logger->log_warn("LaseEdlAcquisitionThread",
503 "Sensor ID is not the expected ID, "
504 "ignoring message");
505 return -1;
506 }
507
508 // two words remaining in first message
509 (*data)[data_index++] = get_word_from_msg(&msg, &msg_index);
510
511 // process all frames
512 for (WORD f = number_of_incoming_frames - 1; f > 0; --f) {
513 msg_index = 0;
514
515 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
516 throw Exception("Laser recv() failed (3)");
517 }
518
519 // get and verify frame number indicator
520 read = get_word_from_msg(&msg, &msg_index);
521 if (read != f) {
522 logger->log_warn("LaseEdlAcquisitionThread",
523 "Recv protocol violation, "
524 "wrong frame number: expected %u, but got %u",
525 f,
526 read);
527 return -1;
528 }
529
530 // process all words in frame
531 number_of_incoming_words = (msg.LEN - msg_index) >> 1;
532 for (int i = 0; i < number_of_incoming_words; ++i) {
533 (*data)[data_index++] = get_word_from_msg(&msg, &msg_index);
534 }
535 }
536
537 // printf("Received (2): "); print_word_array(data_index, *data);
538
539 // might be different from number_of_incoming_words,
540 // since last message can be not full
541 return data_index;
542
543 } else {
544 logger->log_warn("LaseEdlAcquisitionThread",
545 "Recv got strange first response word (neigther 0 nor FFFF)\n");
546 }
547 return -1;
548}
549
550inline void
551LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg)
552{
553 BYTE byte;
554 byte = word >> 8;
555 msg->DATA[(msg->LEN)++] = byte;
556 byte = word;
557 msg->DATA[(msg->LEN)++] = byte;
558}
559
560inline void
561LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg)
562{
563 msg->DATA[(msg->LEN)++] = byte;
564}
565
566inline WORD
567LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg, int *index)
568{
569 WORD rv = msg->DATA[(*index)++] << 8;
570 rv += msg->DATA[((*index)++)];
571 return rv;
572}
573
574WORD *
575LaseEdlAcquisitionThread::make_word_array(int count, ...)
576{
577 va_list word_list;
578 va_start(word_list, count);
579 WORD *rtv;
580 rtv = (WORD *)malloc(sizeof(WORD) * count);
581 for (int i = 0; i < count; ++i) {
582 rtv[i] = (WORD)va_arg(word_list, int);
583 }
584 va_end(word_list);
585 return rtv;
586}
587
588int
589LaseEdlAcquisitionThread::compare_word_arrays(int count, WORD *a, WORD *b)
590{
591 for (int i = 0; i < count; ++i) {
592 if (a[i] != b[i]) {
593 return 0;
594 }
595 }
596 return 1;
597}
598
599void
600LaseEdlAcquisitionThread::print_word_array(int count, WORD *a)
601{
602 for (int i = 0; i < count; ++i) {
603 printf("%04x ", a[i]);
604 }
605 printf("\n");
606}
607
608void
609LaseEdlAcquisitionThread::print_message(TPCANMsg *m)
610{
611 int i;
612 printf("%c %c 0x%08x %1d ",
613 (m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm',
614 (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
615 m->ID,
616 m->LEN);
617
618 for (i = 0; i < m->LEN; i++) {
619 printf("0x%02x ", m->DATA[i]);
620 }
621
622 printf("\n");
623}
624
625void
626LaseEdlAcquisitionThread::process_profiles()
627{
628 WORD *real_response;
629 WORD *expected_response = make_word_array(2, respcode(CMD_GET_PROFILE), cfg_profile_format_);
630 int response_size = recv(&real_response);
631 if (response_size == -1) {
632 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): recv() failed");
633 free(expected_response);
634 return;
635 }
636
637 // wrong answer ?
638 if (!compare_word_arrays(2, real_response, expected_response)) {
639 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): Invalid response received");
640 free(expected_response);
641 free(real_response);
642 return;
643 }
644 // wrong number of values ?
645 if ((response_size - 3 != (int)number_of_values_)
646 && (response_size - 3 != 2 * (int)number_of_values_)) {
647 logger->log_warn("LaseEdlAcquisitionThread",
648 "number of received values "
649 "doesn't match my expectations, recvd %d, expected %d",
650 response_size - 3,
651 number_of_values_);
652 free(expected_response);
653 free(real_response);
654 return;
655 }
656
657 // extract data from response
658 register float dist = 0;
659 register int echo = 0;
660 register int dist_index = (int)roundf(cfg_mount_rotation_ * 16 / cfg_angle_step_);
661 register int echo_index = dist_index;
662
663 _data_mutex->lock();
664 _new_data = true;
665 _timestamp->stamp();
666
667 // see which data is requested
668 if (cfg_profile_format_ == PROFILEFORMAT_DISTANCE) {
669 // only distances
670 for (int i = 3; i < response_size; ++i) {
671 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
672 _distances[number_of_values_ - dist_index] = dist;
673 if (++dist_index >= (int)number_of_values_)
674 dist_index = 0;
675 }
676
677 } else if (cfg_profile_format_ == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE)) {
678 // distances + echos
679 for (int i = 3; i < response_size; ++i) {
680 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
681 _distances[number_of_values_ - dist_index] = dist;
682 if (++dist_index >= (int)number_of_values_)
683 dist_index = 0;
684 ++i;
685 echo = real_response[i];
686 _echoes[number_of_values_ - echo_index] = echo;
687 if (++echo_index >= (int)number_of_values_)
688 echo_index = 0;
689 }
690
691 } else if (cfg_profile_format_ == PROFILEFORMAT_ECHO_AMPLITUDE) {
692 // only echos
693 for (int i = 3; i < response_size; ++i) {
694 echo = real_response[i];
695 _echoes[number_of_values_ - echo_index] = echo;
696 if (++echo_index >= (int)number_of_values_)
697 echo_index = 0;
698 }
699 }
700
702
703 free(real_response);
704 free(expected_response);
705}
706
707void
708LaseEdlAcquisitionThread::send_and_check(WORD * command_data,
709 int command_length,
710 WORD * expected_response,
711 int n,
712 WORD **real_response,
713 int * response_size)
714{
715 bool keep_response = (real_response != NULL);
716 WORD **response;
717 WORD * local_response = NULL;
718 if (keep_response) {
719 response = real_response;
720 } else {
721 response = &local_response;
722 }
723 send(command_data, command_length);
724 int response_s = recv(response);
725
726 if (response_s <= 0) {
727 throw Exception("Did not receive data for command");
728 }
729
730 bool match = compare_word_arrays(n, *response, expected_response);
731
732 if (!match || !keep_response) {
733 free(*response);
734 }
735 free(expected_response);
736 free(command_data);
737
738 if (!match) {
739 throw Exception("Response to query did not match expectation");
740 }
741
742 if (response_size != NULL) {
743 *response_size = response_s;
744 }
745}
746
747void
748LaseEdlAcquisitionThread::SET_CONFIG(WORD config_item, int k, ...)
749{
750 WORD *command;
751 command = (WORD *)malloc(sizeof(WORD) * (2 + k));
752 command[0] = CMD_SET_CONFIG;
753 command[1] = config_item;
754 va_list word_list;
755 va_start(word_list, k);
756 for (int i = 0; i < k; ++i) {
757 command[i + 2] = (WORD)va_arg(word_list, int);
758 }
759 va_end(word_list);
760
761 send_and_check(command, 2 + k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2);
762}
763
764void
765LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func, WORD sect_stop, WORD flash)
766{
767 WORD *command = make_word_array(5, CMD_SET_FUNCTION, sect_num, sect_func, sect_stop, flash);
768 send_and_check(command, 5, make_word_array(2, respcode(CMD_SET_FUNCTION), sect_num), 2);
769}
770
771void
772LaseEdlAcquisitionThread::GET_PROFILE(WORD prof_num, WORD prof_format)
773{
774 WORD *command = make_word_array(3, CMD_GET_PROFILE, prof_num, prof_format);
775 send_and_check(command, 3, make_word_array(2, respcode(CMD_GET_PROFILE), prof_format), 2);
776}
777
778void
779LaseEdlAcquisitionThread::CANCEL_PROFILE()
780{
781 send_and_check(make_word_array(1, CMD_CANCEL_PROFILE),
782 1,
783 make_word_array(1, respcode(CMD_CANCEL_PROFILE)),
784 1);
785}
786
787void
788LaseEdlAcquisitionThread::DO_RESET(WORD reset_level)
789{
790 WORD *command = make_word_array(2, CMD_DO_RESET, reset_level);
791 send_and_check(command, 2, make_word_array(2, respcode(CMD_DO_RESET), reset_level), 2);
792}
793
794void
795LaseEdlAcquisitionThread::TRANS_IDLE()
796{
797 WORD *command = make_word_array(1, CMD_TRANS_IDLE);
798 WORD *real_response;
799 int response_size;
800
801 send_and_check(
802 command, 1, make_word_array(1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size);
803
804 bool failed = (real_response[response_size - 1] != 0x0001);
805 free(real_response);
806 if (failed)
807 throw Exception("Failed to set trans idle");
808}
809
810void
811LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency)
812{
813 WORD *command = make_word_array(2, CMD_TRANS_ROTATE, frequency);
814 WORD *real_response;
815 int response_size;
816 send_and_check(
817 command, 2, make_word_array(1, respcode(CMD_TRANS_ROTATE)), 1, &real_response, &response_size);
818
819 bool failed = (real_response[response_size - 1] != 0x0002);
820 free(real_response);
821 if (failed)
822 throw Exception("Failed to set trans rotate");
823}
824
825void
826LaseEdlAcquisitionThread::TRANS_MEASURE()
827{
828 WORD *command = make_word_array(1, CMD_TRANS_MEASURE);
829 WORD *real_response;
830 int response_size;
831 send_and_check(
832 command, 1, make_word_array(1, respcode(CMD_TRANS_MEASURE)), 1, &real_response, &response_size);
833
834 bool failed =
835 (real_response[response_size - 2] != 0x0003) || (real_response[response_size - 1] != 0x0000);
836 unsigned int error_code = real_response[response_size - 1];
837 free(real_response);
838 if (failed)
839 throw Exception("Failed set trans measure, error code %u", error_code);
840}
LaseEdlAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
virtual void init()
Initialize the thread.
virtual void loop()
Code to execute in the thread.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
virtual void finalize()
Finalize the thread.
Laser acqusition thread.
unsigned int _echoes_size
Assign this the size of the _echoes array.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
bool _new_data
Set to true in your loop if new data is available.
unsigned int _distances_size
Assign this the size of the _distances array.
float * _echoes
Allocate a float array and copy your echo values here.
fawkes::Time * _timestamp
Time when the most recent data was received.
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
Interface for configuration handling.
Definition: config.h:68
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void append(const char *format,...) noexcept
Append messages to the message list.
Definition: exception.cpp:333
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
void lock()
Lock this mutex.
Definition: mutex.cpp:87
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
const char * name() const
Get name of thread.
Definition: thread.h:100
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
Fawkes library namespace.