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 <vector>
29 #include <cstdlib>
30 #include <cmath>
31 #include <string>
32 #include <cstdio>
33 
34 using namespace fawkes;
35 
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;
80 
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;
129 
130 
131 const float LaseEdlAcquisitionThread::DISTANCE_FACTOR = 256.00;
132 
133 
134 /** @class LaseEdlAcquisitionThread "lase_edl_aqt.h"
135  * Laser acqusition thread for Lase EDL L A laser scanner.
136  * This thread fetches the data from the laser.
137  * @author Tim Niemueller
138  * @author Christian Fritz
139  */
140 
141 
142 /** Constructor.
143  * @param cfg_name short name of configuration group
144  * @param cfg_prefix configuration path prefix
145  */
147  std::string &cfg_prefix)
148  : LaserAcquisitionThread("LaseEdlAcquisitionThread")
149 {
150  set_name("LaseEDL(%s)", cfg_name.c_str());
151  __pre_init_done = false;
152  __cfg_name = cfg_name;
153  __cfg_prefix = cfg_prefix;
154 }
155 
156 
157 void
160 {
161  if (__pre_init_done) return;
162 
163  try {
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;
171  } else {
172  logger->log_error(name(), "Canonical resolution %s is invalid, must be 'low' "
173  "or 'high', trying to read raw config data");
174  throw Exception("");
175  }
176  logger->log_debug(name(), "Using canonical resolution %s, freq: %u, angle step: %u",
177  canres.c_str(), __cfg_rotation_freq, __cfg_angle_step);
178  } catch (Exception &e) {
179  // exceptions thrown here will propagate
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());
182  }
183 
184  try {
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());
198 
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;
204  }
205  __number_of_values = 16 * 360 / __cfg_angle_step;
206 
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);
210  }
211 
212  _distances_size = _echoes_size = __number_of_values;
213 
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;
217  } else {
218  throw Exception("Unknown interface type %s", interface_type.c_str());
219  }
220 
221  } catch (Exception &e) {
222  e.append("Could not read all required config values for %s", name());
223  throw;
224  }
225 
226  __pre_init_done = true;
227 }
228 
229 void
231 {
233 
234  init_bus();
235 
236  for (unsigned int i = 1; i <= __cfg_num_init_tries; ++i) {
237 
238  try {
239  CANCEL_PROFILE();
240  } catch (Exception &e) {
241  // ignored, happens often
242  }
243 
244  try {
245  logger->log_debug("LaseEdlAcquisitionThread", "Resetting Laser");
246  DO_RESET(RESETLEVEL_HALT_IDLE);
247 
248  if ( ! __cfg_use_default ) {
249  logger->log_debug("LaseEdlAcquisitionThread", "Setting configuration");
250  // set configuration (rotation and anglestep)
251  SET_CONFIG(CONFIGITEM_GLOBAL, CONFIGDATA_LENGTH_GLOBAL,
252  __cfg_sensor_id, __cfg_rotation_freq, __cfg_angle_step);
253 
254  // set functions (sector definition)
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);
260  }
261 
262  logger->log_debug("LaseEdlAcquisitionThread", "Starting rotating");
263  TRANS_ROTATE(__cfg_rotation_freq);
264  logger->log_debug("LaseEdlAcquisitionThread", "Starting measuring");
265  TRANS_MEASURE();
266  logger->log_debug("LaseEdlAcquisitionThread", "Enable profile retrieval");
267  GET_PROFILE(PROFILENUM_CONTINUOUS, __cfg_profile_format);
268 
269  break; // break for loop if initialization was successful
270  } catch (Exception &e) {
271  if (i < __cfg_num_init_tries) {
272  logger->log_warn("LaseEdlAcquisitionThread", "Initialization, retrying %d more times", __cfg_num_init_tries - i);
273  logger->log_warn("LaseEdlAcquisitionThread", e);
274  } else {
275  logger->log_error("LaseEdlAcquisitionThread", "Initialization failed, giving up after %u tries", __cfg_num_init_tries);
276  throw;
277  }
278  }
279  }
280 
281  _distances = (float *)malloc(sizeof(float) * __number_of_values);
282  _echoes = (float *)malloc(sizeof(float) * __number_of_values);
283 }
284 
285 
286 void
288 {
289  free(_distances);
290  free(_echoes);
291  _distances = _echoes = NULL;
292 
293  logger->log_debug("LaseEdlAcquisitionThread", "Resetting laser");
294  DO_RESET(RESETLEVEL_HALT_IDLE);
295 }
296 
297 
298 void
300 {
301  process_profiles();
302 }
303 
304 
305 unsigned int
306 LaseEdlAcquisitionThread::calc_angle_step(unsigned int rotation_freq,
307  unsigned int max_pulse_freq)
308 {
309  float tmp;
310  unsigned int rv;
311  tmp = ( ((float)max_pulse_freq) / 360.0 ) / ((float)rotation_freq);
312  tmp = ceil( (1 / tmp) * 16.0 );
313  rv = (unsigned int)tmp;
314 
315  if (rv == 7 || rv == 11 || rv == 13 || rv == 14) rv++;
316 
317  return rv;
318 }
319 
320 
321 void
322 LaseEdlAcquisitionThread::init_bus()
323 {
324  FILE *f = fopen("/proc/pcan", "r");
325  if (! f) {
326  throw Exception("Cannot open /proc/pcan, PCAN driver not loaded?");
327  }
328  std::vector<std::string> config_lines;
329  std::vector<std::string> device_lines;
330  char tmp[128];
331  while (fgets(tmp, sizeof(tmp), f)) {
332  if (tmp[0] == '*') {
333  config_lines.push_back(tmp);
334  } else if (tmp[0] != '\n') {
335  device_lines.push_back(tmp);
336  }
337  }
338  fclose(f);
339 
340  std::vector<std::string>::iterator l;
341  for (l = config_lines.begin(); l != config_lines.end(); ++l) {
342  // proc is found
343  std::string::size_type pos = 0;
344  while ((pos = l->find("[", pos)) != std::string::npos) {
345  pos += 1;
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);
349  if (item == "net") {
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.");
353  }
354  }
355  }
356  }
357 
358  __handle = CAN_Open(__cfg_interface_type, 0, __cfg_port, __cfg_irq);
359  if (__handle == NULL) {
360  throw Exception("Cannot open CAN bus");
361  }
362  if (CAN_Init(__handle, __cfg_btr0btr1, CAN_INIT_TYPE_ST) != CAN_ERR_OK) {
363  throw Exception("Cannot initialize CAN bus");
364  }
365 }
366 
367 
368 void
369 LaseEdlAcquisitionThread::send(WORD *data, int n)
370 {
371  TPCANMsg msg;
372  msg.ID = __cfg_can_id;
373  msg.MSGTYPE = MSGTYPE_STANDARD;
374  msg.LEN = 0;
375 
376  int send_words = 0;
377  WORD number_of_frames = 0;
378 
379  // special case for less or equal two words
380  if (n <= 2) {
381  number_of_frames = 1;
382  append_to_msg( (WORD)0, &msg);
383  append_to_msg( (WORD)__cfg_sensor_id, &msg);
384  if (n >= 1) {
385  append_to_msg( data[0], &msg);
386  }
387  if (n == 2) {
388  append_to_msg( data[1], &msg);
389  }
390  //printf("send (1): "); print_message(&msg);
391  if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
392  throw Exception("Laser send() failed (1)");
393  }
394 
395  } else { // more than 2 words
396  number_of_frames = ((n - 1) / 3) + 1;
397  if ((n-1) % 3 != 0) {
398  ++number_of_frames;
399  }
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);
404  // printf("send (2): "); print_message(&msg);
405  if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
406  throw Exception("Laser send() failed (2)");
407  }
408 
409  for (WORD f=number_of_frames-1; f > 1; --f ) {
410  msg.LEN = 0;
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);
415  // printf("send (3): "); print_message(&msg);
416  if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
417  throw Exception("Laser send() failed (3)");
418  }
419  }
420  // last frame
421  msg.LEN = 0;
422  append_to_msg( (WORD)0x0001, &msg);
423  for (int i=send_words; i < n; i++) {
424  append_to_msg( data[send_words++], &msg);
425  }
426  // printf("send (4): "); print_message(&msg);
427  if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
428  throw Exception("Laser send() failed (3)");
429  }
430  }
431 }
432 
433 
434 int
435 LaseEdlAcquisitionThread::recv(WORD **data, bool allocate)
436 {
437  TPCANMsg msg;
438  // read from CAN BUS
439  if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
440  throw Exception("Laser recv() failed (1)");
441  }
442  // If msg wasn't send by our laser: ignore it
443  if (msg.ID != __cfg_can_id_resp) {
444  logger->log_warn("LaseEdlAcquisitionThread", "CAN ID is not the expected ID, "
445  "ignoring message");
446  return -1;
447  }
448 
449  int number_of_incoming_frames = 0;
450  WORD number_of_incoming_words = 0;
451  int msg_index = 0;
452  int data_index = 0;
453  WORD read;
454 
455  read = get_word_from_msg(&msg, &msg_index);
456 
457  // seek for beginning of a block
458  while ((read != 0x0000) && (read != 0xFFFF) ) {
459  if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
460  throw Exception("Laser recv() failed (2)");
461  }
462  msg_index = 0;
463  read = get_word_from_msg( &msg, &msg_index);
464  }
465 
466  // got legal block: process it
467  if (read == 0x0000) { // receiving only one frame
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, "
471  "ignoring message");
472  return -1;
473  }
474  number_of_incoming_words = (msg.LEN - msg_index) / 2;
475  if (allocate) {
476  (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_words));
477  }
478  for (int i=0; i < number_of_incoming_words; ++i) {
479  (*data)[i] = get_word_from_msg( &msg, &msg_index);
480  }
481  // printf("Received (1): "); print_word_array(number_of_incoming_words, *data);
482  return number_of_incoming_words;
483  } else if (read == 0xFFFF) {
484  // get number of incoming frames
485  number_of_incoming_frames = get_word_from_msg( &msg, &msg_index);
486  if (allocate) {
487  (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_frames * 6 + 1));
488  }
489  data_index = 0;
490 
491  // get sensor response ID
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, "
495  "ignoring message");
496  return -1;
497  }
498 
499  // two words remaining in first message
500  (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
501 
502  // process all frames
503  for (WORD f=number_of_incoming_frames-1; f > 0; --f ) {
504  msg_index = 0;
505 
506  if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
507  throw Exception("Laser recv() failed (3)");
508  }
509 
510  // get and verify frame number indicator
511  read = get_word_from_msg( &msg, &msg_index);
512  if (read != f) {
513  logger->log_warn("LaseEdlAcquisitionThread","Recv protocol violation, "
514  "wrong frame number: expected %u, but got %u", f, read);
515  return -1;
516  }
517 
518  // process all words in frame
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);
522  }
523  }
524 
525  // printf("Received (2): "); print_word_array(data_index, *data);
526 
527  // might be different from number_of_incoming_words,
528  // since last message can be not full
529  return data_index;
530 
531  } else {
532  logger->log_warn("LaseEdlAcquisitionThread", "Recv got strange first response word (neigther 0 nor FFFF)\n");
533  }
534  return -1;
535 }
536 
537 
538 inline void
539 LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg)
540 {
541  BYTE byte;
542  byte = word >> 8;
543  msg->DATA[(msg->LEN)++] = byte;
544  byte = word;
545  msg->DATA[(msg->LEN)++] = byte;
546 }
547 
548 
549 inline void
550 LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg)
551 {
552  msg->DATA[(msg->LEN)++] = byte;
553 }
554 
555 inline WORD
556 LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg, int *index)
557 {
558  WORD rv = msg->DATA[(*index)++] << 8;
559  rv += msg->DATA[((*index)++)];
560  return rv;
561 }
562 
563 
564 WORD *
565 LaseEdlAcquisitionThread::make_word_array(int count, ...) {
566  va_list word_list;
567  va_start(word_list, count);
568  WORD *rtv;
569  rtv = (WORD*)malloc( sizeof(WORD) * count);
570  for (int i=0; i<count; ++i) {
571  rtv[i] = (WORD) va_arg(word_list, int);
572  }
573  va_end(word_list);
574  return rtv;
575 }
576 
577 
578 int
579 LaseEdlAcquisitionThread::compare_word_arrays(int count, WORD* a, WORD* b)
580 {
581  for (int i=0; i < count; ++i) {
582  if (a[i] != b[i]) {
583  return 0;
584  }
585  }
586  return 1;
587 }
588 
589 
590 void
591 LaseEdlAcquisitionThread::print_word_array(int count, WORD* a)
592 {
593  for (int i=0; i < count; ++i) {
594  printf("%04x ", a[i]);
595  }
596  printf("\n");
597 }
598 
599 
600 void
601 LaseEdlAcquisitionThread::print_message(TPCANMsg *m)
602 {
603  int i;
604  printf("%c %c 0x%08x %1d ",
605  (m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm',
606  (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
607  m->ID,
608  m->LEN);
609 
610  for (i = 0; i < m->LEN; i++) {
611  printf("0x%02x ", m->DATA[i]);
612  }
613 
614  printf("\n");
615 }
616 
617 void
618 LaseEdlAcquisitionThread::process_profiles()
619 {
620  WORD* real_response;
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");
626  return;
627  }
628 
629  // wrong answer ?
630  if (! compare_word_arrays( 2, real_response, expected_response )) {
631  logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): Invalid response received");
632  return;
633  }
634  // wrong number of values ?
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);
640  return;
641  }
642 
643  // extract data from response
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;
648 
649  _data_mutex->lock();
650  _new_data = true;
651  _timestamp->stamp();
652 
653  // see which data is requested
654  if (__cfg_profile_format == PROFILEFORMAT_DISTANCE ) {
655  // only distances
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;
660  }
661 
662  } else if (__cfg_profile_format == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE) ) {
663  // distances + echos
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;
668  ++i;
669  echo = real_response[i];
670  _echoes[__number_of_values - echo_index] = echo;
671  if (++echo_index >= (int)__number_of_values) echo_index = 0;
672  }
673 
674 
675  } else if (__cfg_profile_format == PROFILEFORMAT_ECHO_AMPLITUDE ) {
676  // only echos
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;
681  }
682  }
683 
684  _data_mutex->unlock();
685 
686  free( real_response );
687  free( expected_response );
688 }
689 
690 
691 void
692 LaseEdlAcquisitionThread::send_and_check(WORD *command_data, int command_length,
693  WORD *expected_response, int n,
694  WORD **real_response, int *response_size)
695 {
696  bool keep_response = (real_response != NULL);
697  WORD **response;
698  WORD *local_response;
699  if (keep_response) {
700  response = real_response;
701  } else {
702  response = &local_response;
703  }
704  send(command_data, command_length);
705  int response_s = recv(response);
706 
707  if (response_s <= 0) {
708  throw Exception("Did not receive data for command");
709  }
710 
711  bool match = compare_word_arrays(n, *response, expected_response);
712 
713  if ( ! match || ! keep_response ) {
714  free(*response);
715  }
716  free(expected_response);
717  free(command_data);
718 
719  if ( ! match) {
720  throw Exception("Response to query did not match expectation");
721  }
722 
723  if ( response_size != NULL ) {
724  *response_size = response_s;
725  }
726 }
727 
728 void
729 LaseEdlAcquisitionThread::SET_CONFIG( WORD config_item, int k, ...)
730 {
731  WORD *command;
732  command = (WORD*)malloc( sizeof(WORD) * (2+k) );
733  command[0] = CMD_SET_CONFIG;
734  command[1] = config_item;
735  va_list word_list;
736  va_start( word_list, k);
737  for (int i=0; i<k; ++i) {
738  command[i+2] = (WORD) va_arg( word_list, int);
739  }
740  va_end( word_list );
741 
742  send_and_check(command, 2+k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2);
743 }
744 
745 
746 void
747 LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func,
748  WORD sect_stop, WORD flash )
749 {
750  WORD* command = make_word_array(5, CMD_SET_FUNCTION, sect_num, sect_func,
751  sect_stop, flash);
752  send_and_check(command, 5, make_word_array(2, respcode(CMD_SET_FUNCTION), sect_num), 2);
753 }
754 
755 
756 void
757 LaseEdlAcquisitionThread::GET_PROFILE( WORD prof_num, WORD prof_format)
758 {
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);
762 }
763 
764 
765 void
766 LaseEdlAcquisitionThread::CANCEL_PROFILE()
767 {
768  send_and_check(make_word_array(1, CMD_CANCEL_PROFILE), 1,
769  make_word_array( 1, respcode(CMD_CANCEL_PROFILE)), 1);
770 }
771 
772 
773 void
774 LaseEdlAcquisitionThread::DO_RESET(WORD reset_level)
775 {
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);
778 }
779 
780 
781 void
782 LaseEdlAcquisitionThread::TRANS_IDLE()
783 {
784  WORD* command = make_word_array( 1, CMD_TRANS_IDLE);
785  WORD* real_response;
786  int response_size;
787 
788  send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size);
789 
790  bool failed = (real_response[response_size-1] != 0x0001);
791  free(real_response);
792  if (failed) throw Exception("Failed to set trans idle");
793 }
794 
795 
796 void
797 LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency)
798 {
799  WORD* command = make_word_array( 2, CMD_TRANS_ROTATE, frequency);
800  WORD* real_response;
801  int response_size;
802  send_and_check(command, 2, make_word_array( 1, respcode(CMD_TRANS_ROTATE)), 1,
803  &real_response, &response_size);
804 
805  bool failed = (real_response[response_size-1] != 0x0002);
806  free(real_response);
807  if ( failed ) throw Exception("Failed to set trans rotate");
808 }
809 
810 
811 void
812 LaseEdlAcquisitionThread::TRANS_MEASURE()
813 {
814  WORD* command = make_word_array( 1, CMD_TRANS_MEASURE);
815  WORD* real_response;
816  int response_size;
817  send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_MEASURE)),
818  1, &real_response, &response_size);
819 
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];
823  free(real_response);
824  if ( failed ) throw Exception("Failed set trans measure, error code %u", error_code);
825 }
LaseEdlAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Laser acqusition thread.
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.
Definition: mutex.cpp:135
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
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.
Definition: thread.cpp:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
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.
Definition: thread.h:95
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.
Definition: mutex.cpp:89
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
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.
Definition: configurable.h:44
Interface for configuration handling.
Definition: config.h:67
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.
Definition: exception.cpp:341
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Interface for logging.
Definition: logger.h:34