24 #include "lase_edl_aqt.h" 26 #include <core/threading/mutex.h> 36 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESET = 0x0000;
37 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESTART = 0x0001;
38 const WORD LaseEdlAcquisitionThread::RESETLEVEL_HALT_IDLE = 0x0002;
39 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RELOAD_VOLTSET = 0x0010;
40 const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET_HISTORIC = 0x0000;
41 const WORD LaseEdlAcquisitionThread::CONFIGITEM_RS232_RS422 = 0x0001;
42 const WORD LaseEdlAcquisitionThread::CONFIGITEM_CAN = 0x0002;
43 const WORD LaseEdlAcquisitionThread::CONFIGITEM_SPI = 0x0003;
44 const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET = 0x0004;
45 const WORD LaseEdlAcquisitionThread::CONFIGITEM_GLOBAL = 0x0010;
46 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_RS232_RS422 = 4;
47 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_CAN = 5;
48 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_ARCNET = 2;
49 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_GLOBAL = 3;
50 const WORD LaseEdlAcquisitionThread::SECTOR_0 = 0x0000;
51 const WORD LaseEdlAcquisitionThread::SECTOR_1 = 0x0001;
52 const WORD LaseEdlAcquisitionThread::SECTOR_2 = 0x0002;
53 const WORD LaseEdlAcquisitionThread::SECTOR_3 = 0x0003;
54 const WORD LaseEdlAcquisitionThread::SECTOR_4 = 0x0004;
55 const WORD LaseEdlAcquisitionThread::SECTOR_5 = 0x0005;
56 const WORD LaseEdlAcquisitionThread::SECTOR_6 = 0x0006;
57 const WORD LaseEdlAcquisitionThread::SECTOR_7 = 0x0007;
58 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NOT_INITIALIZED = 0x0000;
59 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NO_MEASUREMENT = 0x0001;
60 const WORD LaseEdlAcquisitionThread::SECTORFUNC_DUMMY_MEASUREMENT = 0x0002;
61 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NORMAL_MEASUREMENT = 0x0003;
62 const WORD LaseEdlAcquisitionThread::SECTORFUNC_REFERENCE_TARGET = 0x0004;
63 const WORD LaseEdlAcquisitionThread::FLASH_YES = 0x0001;
64 const WORD LaseEdlAcquisitionThread::FLASH_NO = 0x0000;
65 const WORD LaseEdlAcquisitionThread::PROFILENUM_CONTINUOUS = 0x0000;
66 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUMBER = 0x0001;
67 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_COUNTER = 0x0002;
68 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_LAYER = 0x0004;
69 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SECTOR = 0x0008;
70 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ANGLE_STEP = 0x0010;
71 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUM_SECT_POINTS = 0x0020;
72 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_START = 0x0040;
73 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_START_DIRECTION = 0x0080;
74 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DISTANCE = 0x0100;
75 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DIRECTION = 0x0200;
76 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ECHO_AMPLITUDE = 0x0400;
77 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_END = 0x0800;
78 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_END_DIRECTION = 0x1000;
79 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SENSOR_MODE = 0x2000;
81 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_STATUS = 0x0100;
82 const WORD LaseEdlAcquisitionThread::CMD_GET_IDENTIFICATION = 0x0101;
83 const WORD LaseEdlAcquisitionThread::CMD_GET_STATUS = 0x0102;
84 const WORD LaseEdlAcquisitionThread::CMD_GET_ERROR = 0x0103;
85 const WORD LaseEdlAcquisitionThread::CMD_GET_SIGNAL = 0x0104;
86 const WORD LaseEdlAcquisitionThread::CMD_SET_SIGNAL = 0x0105;
87 const WORD LaseEdlAcquisitionThread::CMD_REGISTER_APPLICATION = 0x0106;
88 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_CONFIG = 0x0200;
89 const WORD LaseEdlAcquisitionThread::CMD_SET_CONFIG = 0x0201;
90 const WORD LaseEdlAcquisitionThread::CMD_GET_CONFIG = 0x0202;
91 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_ABS = 0x0203;
92 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_REL = 0x0204;
93 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_CLOCK = 0x0205;
94 const WORD LaseEdlAcquisitionThread::CMD_SET_ZONE = 0x0206;
95 const WORD LaseEdlAcquisitionThread::CMD_GET_ZONE = 0x0207;
96 const WORD LaseEdlAcquisitionThread::CMD_RELEASE_ZONE = 0x0208;
97 const WORD LaseEdlAcquisitionThread::CMD_SET_FILTER = 0x0209;
98 const WORD LaseEdlAcquisitionThread::CMD_SET_FUNCTION = 0x020A;
99 const WORD LaseEdlAcquisitionThread::CMD_GET_FUNCTION = 0x020B;
100 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MEASUREMENT = 0x0300;
101 const WORD LaseEdlAcquisitionThread::CMD_GET_PROFILE = 0x0301;
102 const WORD LaseEdlAcquisitionThread::CMD_CANCEL_PROFILE = 0x0302;
103 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_WORKING = 0x0400;
104 const WORD LaseEdlAcquisitionThread::CMD_DO_RESET = 0x0401;
105 const WORD LaseEdlAcquisitionThread::CMD_TRANS_IDLE = 0x0402;
106 const WORD LaseEdlAcquisitionThread::CMD_TRANS_ROTATE = 0x0403;
107 const WORD LaseEdlAcquisitionThread::CMD_TRANS_MEASURE = 0x0404;
108 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MAINTENANCE = 0x0500;
109 const WORD LaseEdlAcquisitionThread::CMD_DO_ADJUST = 0x0501;
110 const WORD LaseEdlAcquisitionThread::CMD_DO_TEST = 0x0502;
111 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_INTERFACE_ROUTING = 0x0600;
112 const WORD LaseEdlAcquisitionThread::CMD_COM_ATTACH = 0x0601;
113 const WORD LaseEdlAcquisitionThread::CMD_COM_DETACH = 0x0602;
114 const WORD LaseEdlAcquisitionThread::CMD_COM_INIT = 0x0603;
115 const WORD LaseEdlAcquisitionThread::CMD_COM_OUTPUT = 0x0604;
116 const WORD LaseEdlAcquisitionThread::CMD_COM_DATA = 0x0605;
117 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_FILE = 0x0700;
118 const WORD LaseEdlAcquisitionThread::CMD_DIR = 0x0701;
119 const WORD LaseEdlAcquisitionThread::CMD_SAVE = 0x0702;
120 const WORD LaseEdlAcquisitionThread::CMD_LOAD = 0x0703;
121 const WORD LaseEdlAcquisitionThread::CMD_DELETE = 0x0704;
122 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MONITOR = 0x0900;
123 const WORD LaseEdlAcquisitionThread::CMD_MONITOR_ENABLE_LOG = 0x0801;
124 const WORD LaseEdlAcquisitionThread::CMD_MONITOR_DISABLE_LOG = 0x0802;
125 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_ADJUST = 0x7E00;
126 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_SPECIAL = 0x7F00;
127 const WORD LaseEdlAcquisitionThread::CMD_SERVICE_FAILURE = 0x7F00;
128 const WORD LaseEdlAcquisitionThread::RESPONSE_BIT = 0x8000;
131 const float LaseEdlAcquisitionThread::DISTANCE_FACTOR = 256.00;
147 std::string &cfg_prefix)
150 set_name(
"LaseEDL(%s)", cfg_name.c_str());
151 __pre_init_done =
false;
152 __cfg_name = cfg_name;
153 __cfg_prefix = cfg_prefix;
161 if (__pre_init_done)
return;
164 std::string canres = config->
get_string((__cfg_prefix +
"canonical_resolution").c_str());
165 if (canres ==
"low") {
166 __cfg_rotation_freq = 20;
167 __cfg_angle_step = 16;
168 }
else if (canres ==
"high") {
169 __cfg_rotation_freq = 15;
170 __cfg_angle_step = 8;
172 logger->
log_error(
name(),
"Canonical resolution %s is invalid, must be 'low' " 173 "or 'high', trying to read raw config data");
176 logger->
log_debug(
name(),
"Using canonical resolution %s, freq: %u, angle step: %u",
177 canres.c_str(), __cfg_rotation_freq, __cfg_angle_step);
180 __cfg_rotation_freq = config->
get_uint((__cfg_prefix +
"rotation_freq").c_str());
181 __cfg_angle_step = config->
get_uint((__cfg_prefix +
"angle_step").c_str());
185 __cfg_use_default = config->
get_bool((__cfg_prefix +
"use_default").c_str());
186 __cfg_set_default = config->
get_bool((__cfg_prefix +
"set_default").c_str());
187 __cfg_max_pulse_freq = config->
get_uint((__cfg_prefix +
"max_pulse_freq").c_str());
188 __cfg_profile_format = config->
get_uint((__cfg_prefix +
"profile_format").c_str());
189 __cfg_can_id = config->
get_uint((__cfg_prefix +
"can_id").c_str());
190 __cfg_can_id_resp = config->
get_uint((__cfg_prefix +
"can_id_resp").c_str());
191 __cfg_sensor_id = config->
get_uint((__cfg_prefix +
"sensor_id").c_str());
192 __cfg_sensor_id_resp = config->
get_uint((__cfg_prefix +
"sensor_id_resp").c_str());
193 __cfg_btr0btr1 = config->
get_uint((__cfg_prefix +
"btr0btr1").c_str());
194 __cfg_port = config->
get_uint((__cfg_prefix +
"port").c_str());
195 __cfg_irq = config->
get_uint((__cfg_prefix +
"irq").c_str());
196 __cfg_num_init_tries = config->
get_uint((__cfg_prefix +
"num_init_tries").c_str());
197 __cfg_mount_rotation = config->
get_float((__cfg_prefix +
"mount_rotation").c_str());
199 __min_angle_step = calc_angle_step(__cfg_rotation_freq, __cfg_max_pulse_freq);
200 if ( __cfg_angle_step < __min_angle_step ) {
201 logger->
log_warn(
name(),
"Configured angle step %u less than required minimum " 202 "of %u, raising to minimum", __cfg_angle_step, __min_angle_step);
203 __cfg_angle_step = __min_angle_step;
205 __number_of_values = 16 * 360 / __cfg_angle_step;
207 if ( (__number_of_values != 360) && (__number_of_values != 720) ) {
208 throw Exception(
"At the moment only configurations with 360 or 720 " 209 "laser beams are supported, but %u requested", __number_of_values);
214 std::string interface_type = config->
get_string((__cfg_prefix +
"interface_type").c_str());
215 if ( interface_type ==
"usb" ) {
216 __cfg_interface_type = HW_USB;
218 throw Exception(
"Unknown interface type %s", interface_type.c_str());
222 e.
append(
"Could not read all required config values for %s",
name());
226 __pre_init_done =
true;
236 for (
unsigned int i = 1; i <= __cfg_num_init_tries; ++i) {
246 DO_RESET(RESETLEVEL_HALT_IDLE);
248 if ( ! __cfg_use_default ) {
249 logger->
log_debug(
"LaseEdlAcquisitionThread",
"Setting configuration");
251 SET_CONFIG(CONFIGITEM_GLOBAL, CONFIGDATA_LENGTH_GLOBAL,
252 __cfg_sensor_id, __cfg_rotation_freq, __cfg_angle_step);
255 SET_FUNCTION(SECTOR_0, SECTORFUNC_NORMAL_MEASUREMENT,
256 (16 * 360) - __cfg_angle_step,
257 __cfg_set_default ? FLASH_YES : FLASH_NO);
258 SET_FUNCTION(SECTOR_1, SECTORFUNC_NOT_INITIALIZED, 0,
259 __cfg_set_default ? FLASH_YES : FLASH_NO);
263 TRANS_ROTATE(__cfg_rotation_freq);
266 logger->
log_debug(
"LaseEdlAcquisitionThread",
"Enable profile retrieval");
267 GET_PROFILE(PROFILENUM_CONTINUOUS, __cfg_profile_format);
271 if (i < __cfg_num_init_tries) {
272 logger->
log_warn(
"LaseEdlAcquisitionThread",
"Initialization, retrying %d more times", __cfg_num_init_tries - i);
275 logger->
log_error(
"LaseEdlAcquisitionThread",
"Initialization failed, giving up after %u tries", __cfg_num_init_tries);
281 _distances = (
float *)malloc(
sizeof(
float) * __number_of_values);
282 _echoes = (
float *)malloc(
sizeof(
float) * __number_of_values);
294 DO_RESET(RESETLEVEL_HALT_IDLE);
306 LaseEdlAcquisitionThread::calc_angle_step(
unsigned int rotation_freq,
307 unsigned int max_pulse_freq)
311 tmp = ( ((float)max_pulse_freq) / 360.0 ) / ((
float)rotation_freq);
312 tmp = ceil( (1 / tmp) * 16.0 );
313 rv = (
unsigned int)tmp;
315 if (rv == 7 || rv == 11 || rv == 13 || rv == 14) rv++;
322 LaseEdlAcquisitionThread::init_bus()
324 FILE *f = fopen(
"/proc/pcan",
"r");
326 throw Exception(
"Cannot open /proc/pcan, PCAN driver not loaded?");
328 std::vector<std::string> config_lines;
329 std::vector<std::string> device_lines;
331 while (fgets(tmp,
sizeof(tmp), f)) {
333 config_lines.push_back(tmp);
334 }
else if (tmp[0] !=
'\n') {
335 device_lines.push_back(tmp);
340 std::vector<std::string>::iterator l;
341 for (l = config_lines.begin(); l != config_lines.end(); ++l) {
343 std::string::size_type pos = 0;
344 while ((pos = l->find(
"[", pos)) != std::string::npos) {
346 std::string::size_type pos_end = l->find(
"]", pos);
347 if (pos_end != std::string::npos) {
348 std::string item = l->substr(pos, pos_end - pos);
350 throw Exception(
"PCAN driver has been compiled in netdev mode, but " 351 "chardev mode is required. Please read the plugin " 352 "documentation and recompile the PCAN driver.");
358 __handle = CAN_Open(__cfg_interface_type, 0, __cfg_port, __cfg_irq);
359 if (__handle == NULL) {
362 if (CAN_Init(__handle, __cfg_btr0btr1, CAN_INIT_TYPE_ST) != CAN_ERR_OK) {
363 throw Exception(
"Cannot initialize CAN bus");
369 LaseEdlAcquisitionThread::send(WORD *data,
int n)
372 msg.ID = __cfg_can_id;
373 msg.MSGTYPE = MSGTYPE_STANDARD;
377 WORD number_of_frames = 0;
381 number_of_frames = 1;
382 append_to_msg( (WORD)0, &msg);
383 append_to_msg( (WORD)__cfg_sensor_id, &msg);
385 append_to_msg( data[0], &msg);
388 append_to_msg( data[1], &msg);
391 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
392 throw Exception(
"Laser send() failed (1)");
396 number_of_frames = ((n - 1) / 3) + 1;
397 if ((n-1) % 3 != 0) {
400 append_to_msg( (WORD)0xFFFF, &msg);
401 append_to_msg( number_of_frames, &msg);
402 append_to_msg( (WORD)__cfg_sensor_id, &msg);
403 append_to_msg( data[send_words++], &msg);
405 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
406 throw Exception(
"Laser send() failed (2)");
409 for (WORD f=number_of_frames-1; f > 1; --f ) {
411 append_to_msg( f, &msg);
412 append_to_msg( data[send_words++], &msg);
413 append_to_msg( data[send_words++], &msg);
414 append_to_msg( data[send_words++], &msg);
416 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
417 throw Exception(
"Laser send() failed (3)");
422 append_to_msg( (WORD)0x0001, &msg);
423 for (
int i=send_words; i < n; i++) {
424 append_to_msg( data[send_words++], &msg);
427 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
428 throw Exception(
"Laser send() failed (3)");
435 LaseEdlAcquisitionThread::recv(WORD **data,
bool allocate)
439 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
440 throw Exception(
"Laser recv() failed (1)");
443 if (msg.ID != __cfg_can_id_resp) {
444 logger->
log_warn(
"LaseEdlAcquisitionThread",
"CAN ID is not the expected ID, " 449 int number_of_incoming_frames = 0;
450 WORD number_of_incoming_words = 0;
455 read = get_word_from_msg(&msg, &msg_index);
458 while ((read != 0x0000) && (read != 0xFFFF) ) {
459 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
460 throw Exception(
"Laser recv() failed (2)");
463 read = get_word_from_msg( &msg, &msg_index);
467 if (read == 0x0000) {
468 read = get_word_from_msg( &msg, &msg_index);
469 if (read != __cfg_sensor_id_resp) {
470 logger->
log_warn(
"LaseEdlAcquisitionThread",
"Sensor ID is not the expected ID, " 474 number_of_incoming_words = (msg.LEN - msg_index) / 2;
476 (*data) = (WORD*)malloc(
sizeof(WORD)* (number_of_incoming_words));
478 for (
int i=0; i < number_of_incoming_words; ++i) {
479 (*data)[i] = get_word_from_msg( &msg, &msg_index);
482 return number_of_incoming_words;
483 }
else if (read == 0xFFFF) {
485 number_of_incoming_frames = get_word_from_msg( &msg, &msg_index);
487 (*data) = (WORD*)malloc(
sizeof(WORD)* (number_of_incoming_frames * 6 + 1));
492 read = get_word_from_msg( &msg, &msg_index);
493 if (read != __cfg_sensor_id_resp) {
494 logger->
log_warn(
"LaseEdlAcquisitionThread",
"Sensor ID is not the expected ID, " 500 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
503 for (WORD f=number_of_incoming_frames-1; f > 0; --f ) {
506 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
507 throw Exception(
"Laser recv() failed (3)");
511 read = get_word_from_msg( &msg, &msg_index);
513 logger->
log_warn(
"LaseEdlAcquisitionThread",
"Recv protocol violation, " 514 "wrong frame number: expected %u, but got %u", f, read);
519 number_of_incoming_words = (msg.LEN - msg_index) >> 1;
520 for (
int i=0; i < number_of_incoming_words; ++i) {
521 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
532 logger->
log_warn(
"LaseEdlAcquisitionThread",
"Recv got strange first response word (neigther 0 nor FFFF)\n");
539 LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg)
543 msg->DATA[(msg->LEN)++] = byte;
545 msg->DATA[(msg->LEN)++] = byte;
550 LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg)
552 msg->DATA[(msg->LEN)++] = byte;
556 LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg,
int *index)
558 WORD rv = msg->DATA[(*index)++] << 8;
559 rv += msg->DATA[((*index)++)];
565 LaseEdlAcquisitionThread::make_word_array(
int count, ...) {
567 va_start(word_list, count);
569 rtv = (WORD*)malloc(
sizeof(WORD) * count);
570 for (
int i=0; i<count; ++i) {
571 rtv[i] = (WORD) va_arg(word_list,
int);
579 LaseEdlAcquisitionThread::compare_word_arrays(
int count, WORD* a, WORD* b)
581 for (
int i=0; i < count; ++i) {
591 LaseEdlAcquisitionThread::print_word_array(
int count, WORD* a)
593 for (
int i=0; i < count; ++i) {
594 printf(
"%04x ", a[i]);
601 LaseEdlAcquisitionThread::print_message(TPCANMsg *m)
604 printf(
"%c %c 0x%08x %1d ",
605 (m->MSGTYPE & MSGTYPE_RTR) ?
'r' :
'm',
606 (m->MSGTYPE & MSGTYPE_EXTENDED) ?
'e' :
's',
610 for (i = 0; i < m->LEN; i++) {
611 printf(
"0x%02x ", m->DATA[i]);
618 LaseEdlAcquisitionThread::process_profiles()
621 WORD* expected_response = make_word_array( 2, respcode(CMD_GET_PROFILE),
622 __cfg_profile_format);
623 int response_size = recv(&real_response);
624 if (response_size == -1) {
625 logger->
log_warn(
"LaseEdlAcquisitionThread",
"process_profiles(): recv() failed");
630 if (! compare_word_arrays( 2, real_response, expected_response )) {
631 logger->
log_warn(
"LaseEdlAcquisitionThread",
"process_profiles(): Invalid response received");
635 if ( (response_size - 3 != (
int)__number_of_values) &&
636 (response_size - 3 != 2 * (
int)__number_of_values) ) {
637 logger->
log_warn(
"LaseEdlAcquisitionThread",
"number of received values " 638 "doesn't match my expectations, recvd %d, expected %d",
639 response_size - 3, __number_of_values);
644 register float dist = 0;
645 register int echo = 0;
646 register int dist_index = (int)roundf(__cfg_mount_rotation * 16 / __cfg_angle_step);
647 register int echo_index = dist_index;
654 if (__cfg_profile_format == PROFILEFORMAT_DISTANCE ) {
656 for (
int i=3; i < response_size; ++i ) {
657 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
658 _distances[__number_of_values - dist_index] = dist;
659 if (++dist_index >= (
int)__number_of_values) dist_index = 0;
662 }
else if (__cfg_profile_format == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE) ) {
664 for (
int i=3; i < response_size; ++i) {
665 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
666 _distances[__number_of_values - dist_index] = dist;
667 if (++dist_index >= (
int)__number_of_values) dist_index = 0;
669 echo = real_response[i];
670 _echoes[__number_of_values - echo_index] = echo;
671 if (++echo_index >= (
int)__number_of_values) echo_index = 0;
675 }
else if (__cfg_profile_format == PROFILEFORMAT_ECHO_AMPLITUDE ) {
677 for (
int i=3; i < response_size; ++i ) {
678 echo = real_response[i];
679 _echoes[__number_of_values - echo_index] = echo;
680 if (++echo_index >= (
int)__number_of_values) echo_index = 0;
686 free( real_response );
687 free( expected_response );
692 LaseEdlAcquisitionThread::send_and_check(WORD *command_data,
int command_length,
693 WORD *expected_response,
int n,
694 WORD **real_response,
int *response_size)
696 bool keep_response = (real_response != NULL);
698 WORD *local_response;
700 response = real_response;
702 response = &local_response;
704 send(command_data, command_length);
705 int response_s = recv(response);
707 if (response_s <= 0) {
708 throw Exception(
"Did not receive data for command");
711 bool match = compare_word_arrays(n, *response, expected_response);
713 if ( ! match || ! keep_response ) {
716 free(expected_response);
720 throw Exception(
"Response to query did not match expectation");
723 if ( response_size != NULL ) {
724 *response_size = response_s;
729 LaseEdlAcquisitionThread::SET_CONFIG( WORD config_item,
int k, ...)
732 command = (WORD*)malloc(
sizeof(WORD) * (2+k) );
733 command[0] = CMD_SET_CONFIG;
734 command[1] = config_item;
736 va_start( word_list, k);
737 for (
int i=0; i<k; ++i) {
738 command[i+2] = (WORD) va_arg( word_list,
int);
742 send_and_check(command, 2+k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2);
747 LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func,
748 WORD sect_stop, WORD flash )
750 WORD* command = make_word_array(5, CMD_SET_FUNCTION, sect_num, sect_func,
752 send_and_check(command, 5, make_word_array(2, respcode(CMD_SET_FUNCTION), sect_num), 2);
757 LaseEdlAcquisitionThread::GET_PROFILE( WORD prof_num, WORD prof_format)
759 WORD* command = make_word_array(3, CMD_GET_PROFILE, prof_num, prof_format);
760 send_and_check(command, 3,
761 make_word_array(2, respcode(CMD_GET_PROFILE), prof_format), 2);
766 LaseEdlAcquisitionThread::CANCEL_PROFILE()
768 send_and_check(make_word_array(1, CMD_CANCEL_PROFILE), 1,
769 make_word_array( 1, respcode(CMD_CANCEL_PROFILE)), 1);
774 LaseEdlAcquisitionThread::DO_RESET(WORD reset_level)
776 WORD* command = make_word_array( 2, CMD_DO_RESET, reset_level);
777 send_and_check(command, 2, make_word_array(2, respcode(CMD_DO_RESET), reset_level), 2);
782 LaseEdlAcquisitionThread::TRANS_IDLE()
784 WORD* command = make_word_array( 1, CMD_TRANS_IDLE);
788 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size);
790 bool failed = (real_response[response_size-1] != 0x0001);
792 if (failed)
throw Exception(
"Failed to set trans idle");
797 LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency)
799 WORD* command = make_word_array( 2, CMD_TRANS_ROTATE, frequency);
802 send_and_check(command, 2, make_word_array( 1, respcode(CMD_TRANS_ROTATE)), 1,
803 &real_response, &response_size);
805 bool failed = (real_response[response_size-1] != 0x0002);
807 if ( failed )
throw Exception(
"Failed to set trans rotate");
812 LaseEdlAcquisitionThread::TRANS_MEASURE()
814 WORD* command = make_word_array( 1, CMD_TRANS_MEASURE);
817 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_MEASURE)),
818 1, &real_response, &response_size);
820 bool failed = (real_response[response_size-2] != 0x0003) ||
821 (real_response[response_size-1] != 0x0000);
822 unsigned int error_code = real_response[response_size-1];
824 if ( failed )
throw Exception(
"Failed set trans measure, error code %u", error_code);
LaseEdlAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
void unlock()
Unlock the mutex.
Logger * logger
This is the Logger member used to access the logger.
fawkes::Time * _timestamp
Time when the most recent data was received.
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
virtual void loop()
Code to execute in the thread.
void set_name(const char *format,...)
Set name of thread.
Base class for exceptions in Fawkes.
unsigned int _distances_size
Assign this the size of the _distances array.
float * _distances
Allocate a float array and copy your distance values measured in meters here.
const char * name() const
Get name of thread.
unsigned int _echoes_size
Assign this the size of the _echoes array.
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.
bool _new_data
Set to true in your loop if new data is available.
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void lock()
Lock this mutex.
Time & stamp()
Set this time to the current time.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
virtual void finalize()
Finalize the thread.
Configuration * config
This is the Configuration member used to access the configuration.
Interface for configuration handling.
virtual void init()
Initialize the thread.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
float * _echoes
Allocate a float array and copy your echo values here.
void append(const char *format,...)
Append messages to the message list.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.