24#include "lase_edl_aqt.h"
26#include <core/threading/mutex.h>
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;
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;
130const float LaseEdlAcquisitionThread::DISTANCE_FACTOR = 256.00;
146 set_name(
"LaseEDL(%s)", cfg_name.c_str());
147 pre_init_done_ =
false;
148 cfg_name_ = cfg_name;
149 cfg_prefix_ = cfg_prefix;
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;
168 "Canonical resolution %s is invalid, must be 'low' "
169 "or 'high', trying to read raw config data");
173 "Using canonical resolution %s, freq: %u, angle step: %u",
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());
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());
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());
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());
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",
205 cfg_angle_step_ = min_angle_step_;
207 number_of_values_ = 16 * 360 / cfg_angle_step_;
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",
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;
221 throw Exception(
"Unknown interface type %s", interface_type.c_str());
225 e.
append(
"Could not read all required config values for %s",
name());
229 pre_init_done_ =
true;
239 for (
unsigned int i = 1; i <= cfg_num_init_tries_; ++i) {
248 DO_RESET(RESETLEVEL_HALT_IDLE);
250 if (!cfg_use_default_) {
251 logger->
log_debug(
"LaseEdlAcquisitionThread",
"Setting configuration");
253 SET_CONFIG(CONFIGITEM_GLOBAL,
254 CONFIGDATA_LENGTH_GLOBAL,
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,
267 cfg_set_default_ ? FLASH_YES : FLASH_NO);
271 TRANS_ROTATE(cfg_rotation_freq_);
274 logger->
log_debug(
"LaseEdlAcquisitionThread",
"Enable profile retrieval");
275 GET_PROFILE(PROFILENUM_CONTINUOUS, cfg_profile_format_);
279 if (i < cfg_num_init_tries_) {
281 "Initialization, retrying %d more times",
282 cfg_num_init_tries_ - i);
286 "Initialization failed, giving up after %u tries",
287 cfg_num_init_tries_);
293 _distances = (
float *)malloc(
sizeof(
float) * number_of_values_);
294 _echoes = (
float *)malloc(
sizeof(
float) * number_of_values_);
305 DO_RESET(RESETLEVEL_HALT_IDLE);
315LaseEdlAcquisitionThread::calc_angle_step(
unsigned int rotation_freq,
unsigned int max_pulse_freq)
319 tmp = (((float)max_pulse_freq) / 360.0) / ((
float)rotation_freq);
320 tmp = ceil((1 / tmp) * 16.0);
321 rv = (
unsigned int)tmp;
323 if (rv == 7 || rv == 11 || rv == 13 || rv == 14)
330LaseEdlAcquisitionThread::init_bus()
332 FILE *f = fopen(
"/proc/pcan",
"r");
334 throw Exception(
"Cannot open /proc/pcan, PCAN driver not loaded?");
336 std::vector<std::string> config_lines;
337 std::vector<std::string> device_lines;
339 while (fgets(tmp,
sizeof(tmp), f)) {
341 config_lines.push_back(tmp);
342 }
else if (tmp[0] !=
'\n') {
343 device_lines.push_back(tmp);
348 std::vector<std::string>::iterator l;
349 for (l = config_lines.begin(); l != config_lines.end(); ++l) {
351 std::string::size_type pos = 0;
352 while ((pos = l->find(
"[", pos)) != std::string::npos) {
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);
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.");
366 handle_ = CAN_Open(cfg_interface_type_, 0, cfg_port_, cfg_irq_);
367 if (handle_ == NULL) {
370 if (CAN_Init(handle_, cfg_btr0btr1_, CAN_INIT_TYPE_ST) != CAN_ERR_OK) {
371 throw Exception(
"Cannot initialize CAN bus");
376LaseEdlAcquisitionThread::send(WORD *data,
int n)
379 msg.ID = cfg_can_id_;
380 msg.MSGTYPE = MSGTYPE_STANDARD;
383 WORD number_of_frames = 0;
387 number_of_frames = 1;
388 append_to_msg((WORD)0, &msg);
389 append_to_msg((WORD)cfg_sensor_id_, &msg);
391 append_to_msg(data[0], &msg);
394 append_to_msg(data[1], &msg);
397 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
398 throw Exception(
"Laser send() failed (1)");
403 number_of_frames = ((n - 1) / 3) + 1;
404 if ((n - 1) % 3 != 0) {
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);
412 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
413 throw Exception(
"Laser send() failed (2)");
416 for (WORD f = number_of_frames - 1; f > 1; --f) {
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);
423 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
424 throw Exception(
"Laser send() failed (3)");
429 append_to_msg((WORD)0x0001, &msg);
430 for (
int i = sent_words; i < n; i++) {
431 append_to_msg(data[sent_words++], &msg);
434 if (CAN_Write(handle_, &msg) != CAN_ERR_OK) {
435 throw Exception(
"Laser send() failed (3)");
441LaseEdlAcquisitionThread::recv(WORD **data,
bool allocate)
445 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
446 throw Exception(
"Laser recv() failed (1)");
449 if (msg.ID != cfg_can_id_resp_) {
451 "CAN ID is not the expected ID, "
456 int number_of_incoming_frames = 0;
457 WORD number_of_incoming_words = 0;
462 read = get_word_from_msg(&msg, &msg_index);
465 while ((read != 0x0000) && (read != 0xFFFF)) {
466 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
467 throw Exception(
"Laser recv() failed (2)");
470 read = get_word_from_msg(&msg, &msg_index);
474 if (read == 0x0000) {
475 read = get_word_from_msg(&msg, &msg_index);
476 if (read != cfg_sensor_id_resp_) {
478 "Sensor ID is not the expected ID, "
482 number_of_incoming_words = (msg.LEN - msg_index) / 2;
484 (*data) = (WORD *)malloc(
sizeof(WORD) * (number_of_incoming_words));
486 for (
int i = 0; i < number_of_incoming_words; ++i) {
487 (*data)[i] = get_word_from_msg(&msg, &msg_index);
490 return number_of_incoming_words;
491 }
else if (read == 0xFFFF) {
493 number_of_incoming_frames = get_word_from_msg(&msg, &msg_index);
495 (*data) = (WORD *)malloc(
sizeof(WORD) * (number_of_incoming_frames * 6 + 1));
500 read = get_word_from_msg(&msg, &msg_index);
501 if (read != cfg_sensor_id_resp_) {
503 "Sensor ID is not the expected ID, "
509 (*data)[data_index++] = get_word_from_msg(&msg, &msg_index);
512 for (WORD f = number_of_incoming_frames - 1; f > 0; --f) {
515 if (CAN_Read(handle_, &msg) != CAN_ERR_OK) {
516 throw Exception(
"Laser recv() failed (3)");
520 read = get_word_from_msg(&msg, &msg_index);
523 "Recv protocol violation, "
524 "wrong frame number: expected %u, but got %u",
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);
545 "Recv got strange first response word (neigther 0 nor FFFF)\n");
551LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg)
555 msg->DATA[(msg->LEN)++] =
byte;
557 msg->DATA[(msg->LEN)++] =
byte;
561LaseEdlAcquisitionThread::append_to_msg(BYTE
byte, TPCANMsg *msg)
563 msg->DATA[(msg->LEN)++] =
byte;
567LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg,
int *index)
569 WORD rv = msg->DATA[(*index)++] << 8;
570 rv += msg->DATA[((*index)++)];
575LaseEdlAcquisitionThread::make_word_array(
int count, ...)
578 va_start(word_list, count);
580 rtv = (WORD *)malloc(
sizeof(WORD) * count);
581 for (
int i = 0; i < count; ++i) {
582 rtv[i] = (WORD)va_arg(word_list,
int);
589LaseEdlAcquisitionThread::compare_word_arrays(
int count, WORD *a, WORD *b)
591 for (
int i = 0; i < count; ++i) {
600LaseEdlAcquisitionThread::print_word_array(
int count, WORD *a)
602 for (
int i = 0; i < count; ++i) {
603 printf(
"%04x ", a[i]);
609LaseEdlAcquisitionThread::print_message(TPCANMsg *m)
612 printf(
"%c %c 0x%08x %1d ",
613 (m->MSGTYPE & MSGTYPE_RTR) ?
'r' :
'm',
614 (m->MSGTYPE & MSGTYPE_EXTENDED) ?
'e' :
's',
618 for (i = 0; i < m->LEN; i++) {
619 printf(
"0x%02x ", m->DATA[i]);
626LaseEdlAcquisitionThread::process_profiles()
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);
638 if (!compare_word_arrays(2, real_response, expected_response)) {
639 logger->
log_warn(
"LaseEdlAcquisitionThread",
"process_profiles(): Invalid response received");
640 free(expected_response);
645 if ((response_size - 3 != (
int)number_of_values_)
646 && (response_size - 3 != 2 * (
int)number_of_values_)) {
648 "number of received values "
649 "doesn't match my expectations, recvd %d, expected %d",
652 free(expected_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;
668 if (cfg_profile_format_ == PROFILEFORMAT_DISTANCE) {
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_)
677 }
else if (cfg_profile_format_ == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE)) {
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_)
685 echo = real_response[i];
686 _echoes[number_of_values_ - echo_index] = echo;
687 if (++echo_index >= (
int)number_of_values_)
691 }
else if (cfg_profile_format_ == PROFILEFORMAT_ECHO_AMPLITUDE) {
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_)
704 free(expected_response);
708LaseEdlAcquisitionThread::send_and_check(WORD * command_data,
710 WORD * expected_response,
712 WORD **real_response,
715 bool keep_response = (real_response != NULL);
717 WORD * local_response = NULL;
719 response = real_response;
721 response = &local_response;
723 send(command_data, command_length);
724 int response_s = recv(response);
726 if (response_s <= 0) {
727 throw Exception(
"Did not receive data for command");
730 bool match = compare_word_arrays(n, *response, expected_response);
732 if (!match || !keep_response) {
735 free(expected_response);
739 throw Exception(
"Response to query did not match expectation");
742 if (response_size != NULL) {
743 *response_size = response_s;
748LaseEdlAcquisitionThread::SET_CONFIG(WORD config_item,
int k, ...)
751 command = (WORD *)malloc(
sizeof(WORD) * (2 + k));
752 command[0] = CMD_SET_CONFIG;
753 command[1] = config_item;
755 va_start(word_list, k);
756 for (
int i = 0; i < k; ++i) {
757 command[i + 2] = (WORD)va_arg(word_list,
int);
761 send_and_check(command, 2 + k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2);
765LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func, WORD sect_stop, WORD flash)
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);
772LaseEdlAcquisitionThread::GET_PROFILE(WORD prof_num, WORD prof_format)
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);
779LaseEdlAcquisitionThread::CANCEL_PROFILE()
781 send_and_check(make_word_array(1, CMD_CANCEL_PROFILE),
783 make_word_array(1, respcode(CMD_CANCEL_PROFILE)),
788LaseEdlAcquisitionThread::DO_RESET(WORD reset_level)
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);
795LaseEdlAcquisitionThread::TRANS_IDLE()
797 WORD *command = make_word_array(1, CMD_TRANS_IDLE);
802 command, 1, make_word_array(1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size);
804 bool failed = (real_response[response_size - 1] != 0x0001);
807 throw Exception(
"Failed to set trans idle");
811LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency)
813 WORD *command = make_word_array(2, CMD_TRANS_ROTATE, frequency);
817 command, 2, make_word_array(1, respcode(CMD_TRANS_ROTATE)), 1, &real_response, &response_size);
819 bool failed = (real_response[response_size - 1] != 0x0002);
822 throw Exception(
"Failed to set trans rotate");
826LaseEdlAcquisitionThread::TRANS_MEASURE()
828 WORD *command = make_word_array(1, CMD_TRANS_MEASURE);
832 command, 1, make_word_array(1, respcode(CMD_TRANS_MEASURE)), 1, &real_response, &response_size);
835 (real_response[response_size - 2] != 0x0003) || (real_response[response_size - 1] != 0x0000);
836 unsigned int error_code = real_response[response_size - 1];
839 throw Exception(
"Failed set trans measure, error code %u", error_code);
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.
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.
Interface for configuration handling.
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.
void append(const char *format,...) noexcept
Append messages to the message list.
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.
void lock()
Lock this mutex.
void unlock()
Unlock the mutex.
const char * name() const
Get name of thread.
void set_name(const char *format,...)
Set name of thread.
Time & stamp()
Set this time to the current time.
Fawkes library namespace.