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 
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 const 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  */
143 LaseEdlAcquisitionThread::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 
152 void
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 {
167  logger->log_error(name(),
168  "Canonical resolution %s is invalid, must be 'low' "
169  "or 'high', trying to read raw config data");
170  throw Exception("");
171  }
172  logger->log_debug(name(),
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_) {
200  logger->log_warn(name(),
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 
232 void
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 
297 void
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 
308 void
310 {
311  process_profiles();
312 }
313 
314 unsigned int
315 LaseEdlAcquisitionThread::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 
329 void
330 LaseEdlAcquisitionThread::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 
375 void
376 LaseEdlAcquisitionThread::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 
440 int
441 LaseEdlAcquisitionThread::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 
550 inline void
551 LaseEdlAcquisitionThread::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 
560 inline void
561 LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg)
562 {
563  msg->DATA[(msg->LEN)++] = byte;
564 }
565 
566 inline WORD
567 LaseEdlAcquisitionThread::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 
574 WORD *
575 LaseEdlAcquisitionThread::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 
588 int
589 LaseEdlAcquisitionThread::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 
599 void
600 LaseEdlAcquisitionThread::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 
608 void
609 LaseEdlAcquisitionThread::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 
625 void
626 LaseEdlAcquisitionThread::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 
701  _data_mutex->unlock();
702 
703  free(real_response);
704  free(expected_response);
705 }
706 
707 void
708 LaseEdlAcquisitionThread::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 
747 void
748 LaseEdlAcquisitionThread::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 
764 void
765 LaseEdlAcquisitionThread::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 
771 void
772 LaseEdlAcquisitionThread::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 
778 void
779 LaseEdlAcquisitionThread::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 
787 void
788 LaseEdlAcquisitionThread::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 
794 void
795 LaseEdlAcquisitionThread::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 
810 void
811 LaseEdlAcquisitionThread::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 
825 void
826 LaseEdlAcquisitionThread::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 }
fawkes::Mutex::lock
void lock()
Lock this mutex.
Definition: mutex.cpp:87
LaserAcquisitionThread
Laser acqusition thread.
Definition: acquisition_thread.h:42
LaserAcquisitionThread::_distances_size
unsigned int _distances_size
Assign this the size of the _distances array.
Definition: acquisition_thread.h:81
fawkes::Configuration::get_bool
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
LaserAcquisitionThread::_data_mutex
fawkes::Mutex * _data_mutex
Lock while writing to distances or echoes array or marking new data.
Definition: acquisition_thread.h:74
LaserAcquisitionThread::_echoes
float * _echoes
Allocate a float array and copy your echo values here.
Definition: acquisition_thread.h:79
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::Mutex::unlock
void unlock()
Unlock the mutex.
Definition: mutex.cpp:131
LaserAcquisitionThread::_timestamp
fawkes::Time * _timestamp
Time when the most recent data was received.
Definition: acquisition_thread.h:75
LaseEdlAcquisitionThread::loop
virtual void loop()
Code to execute in the thread.
Definition: lase_edl_aqt.cpp:309
LaseEdlAcquisitionThread::finalize
virtual void finalize()
Finalize the thread.
Definition: lase_edl_aqt.cpp:298
fawkes::Configuration
Interface for configuration handling.
Definition: config.h:65
fawkes::Exception::append
void append(const char *format,...)
Append messages to the message list.
Definition: exception.cpp:333
LaseEdlAcquisitionThread::pre_init
virtual void pre_init(fawkes::Configuration *config, fawkes::Logger *logger)
Pre initialization.
Definition: lase_edl_aqt.cpp:153
LaserAcquisitionThread::_new_data
bool _new_data
Set to true in your loop if new data is available.
Definition: acquisition_thread.h:77
LaseEdlAcquisitionThread::init
virtual void init()
Initialize the thread.
Definition: lase_edl_aqt.cpp:233
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes
Fawkes library namespace.
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
LaserAcquisitionThread::_echoes_size
unsigned int _echoes_size
Assign this the size of the _echoes array.
Definition: acquisition_thread.h:82
LaserAcquisitionThread::_distances
float * _distances
Allocate a float array and copy your distance values measured in meters here.
Definition: acquisition_thread.h:78
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Configuration::get_uint
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
LaseEdlAcquisitionThread::LaseEdlAcquisitionThread
LaseEdlAcquisitionThread(std::string &cfg_name, std::string &cfg_prefix)
Constructor.
Definition: lase_edl_aqt.cpp:143
fawkes::Thread::set_name
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36