Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * lase_edl_aqt.cpp - Thread that retrieves the laser data 00004 * 00005 * Created: Wed Oct 08 13:42:32 2008 00006 * Copyright 2002 Christian Fritz 00007 * 2008-2009 Tim Niemueller [www.niemueller.de] 00008 * 00009 ****************************************************************************/ 00010 00011 /* This program is free software; you can redistribute it and/or modify 00012 * it under the terms of the GNU General Public License as published by 00013 * the Free Software Foundation; either version 2 of the License, or 00014 * (at your option) any later version. 00015 * 00016 * This program is distributed in the hope that it will be useful, 00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 * GNU Library General Public License for more details. 00020 * 00021 * Read the full text in the LICENSE.GPL file in the doc directory. 00022 */ 00023 00024 #include "lase_edl_aqt.h" 00025 00026 #include <core/threading/mutex.h> 00027 00028 #include <vector> 00029 #include <cstdlib> 00030 #include <cmath> 00031 #include <string> 00032 #include <cstdio> 00033 00034 using namespace fawkes; 00035 00036 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESET = 0x0000; 00037 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RESTART = 0x0001; 00038 const WORD LaseEdlAcquisitionThread::RESETLEVEL_HALT_IDLE = 0x0002; 00039 const WORD LaseEdlAcquisitionThread::RESETLEVEL_RELOAD_VOLTSET = 0x0010; 00040 const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET_HISTORIC = 0x0000; 00041 const WORD LaseEdlAcquisitionThread::CONFIGITEM_RS232_RS422 = 0x0001; 00042 const WORD LaseEdlAcquisitionThread::CONFIGITEM_CAN = 0x0002; 00043 const WORD LaseEdlAcquisitionThread::CONFIGITEM_SPI = 0x0003; 00044 const WORD LaseEdlAcquisitionThread::CONFIGITEM_ARCNET = 0x0004; 00045 const WORD LaseEdlAcquisitionThread::CONFIGITEM_GLOBAL = 0x0010; 00046 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_RS232_RS422 = 4; 00047 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_CAN = 5; 00048 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_ARCNET = 2; 00049 const WORD LaseEdlAcquisitionThread::CONFIGDATA_LENGTH_GLOBAL = 3; 00050 const WORD LaseEdlAcquisitionThread::SECTOR_0 = 0x0000; 00051 const WORD LaseEdlAcquisitionThread::SECTOR_1 = 0x0001; 00052 const WORD LaseEdlAcquisitionThread::SECTOR_2 = 0x0002; 00053 const WORD LaseEdlAcquisitionThread::SECTOR_3 = 0x0003; 00054 const WORD LaseEdlAcquisitionThread::SECTOR_4 = 0x0004; 00055 const WORD LaseEdlAcquisitionThread::SECTOR_5 = 0x0005; 00056 const WORD LaseEdlAcquisitionThread::SECTOR_6 = 0x0006; 00057 const WORD LaseEdlAcquisitionThread::SECTOR_7 = 0x0007; 00058 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NOT_INITIALIZED = 0x0000; 00059 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NO_MEASUREMENT = 0x0001; 00060 const WORD LaseEdlAcquisitionThread::SECTORFUNC_DUMMY_MEASUREMENT = 0x0002; 00061 const WORD LaseEdlAcquisitionThread::SECTORFUNC_NORMAL_MEASUREMENT = 0x0003; 00062 const WORD LaseEdlAcquisitionThread::SECTORFUNC_REFERENCE_TARGET = 0x0004; 00063 const WORD LaseEdlAcquisitionThread::FLASH_YES = 0x0001; 00064 const WORD LaseEdlAcquisitionThread::FLASH_NO = 0x0000; 00065 const WORD LaseEdlAcquisitionThread::PROFILENUM_CONTINUOUS = 0x0000; 00066 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUMBER = 0x0001; 00067 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_COUNTER = 0x0002; 00068 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_LAYER = 0x0004; 00069 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SECTOR = 0x0008; 00070 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ANGLE_STEP = 0x0010; 00071 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_NUM_SECT_POINTS = 0x0020; 00072 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_START = 0x0040; 00073 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_START_DIRECTION = 0x0080; 00074 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DISTANCE = 0x0100; 00075 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_DIRECTION = 0x0200; 00076 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_ECHO_AMPLITUDE = 0x0400; 00077 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_TIMESTAMP_END = 0x0800; 00078 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_END_DIRECTION = 0x1000; 00079 const WORD LaseEdlAcquisitionThread::PROFILEFORMAT_SENSOR_MODE = 0x2000; 00080 00081 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_STATUS = 0x0100; 00082 const WORD LaseEdlAcquisitionThread::CMD_GET_IDENTIFICATION = 0x0101; 00083 const WORD LaseEdlAcquisitionThread::CMD_GET_STATUS = 0x0102; 00084 const WORD LaseEdlAcquisitionThread::CMD_GET_ERROR = 0x0103; 00085 const WORD LaseEdlAcquisitionThread::CMD_GET_SIGNAL = 0x0104; 00086 const WORD LaseEdlAcquisitionThread::CMD_SET_SIGNAL = 0x0105; 00087 const WORD LaseEdlAcquisitionThread::CMD_REGISTER_APPLICATION = 0x0106; 00088 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_CONFIG = 0x0200; 00089 const WORD LaseEdlAcquisitionThread::CMD_SET_CONFIG = 0x0201; 00090 const WORD LaseEdlAcquisitionThread::CMD_GET_CONFIG = 0x0202; 00091 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_ABS = 0x0203; 00092 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_REL = 0x0204; 00093 const WORD LaseEdlAcquisitionThread::CMD_SET_SYNC_CLOCK = 0x0205; 00094 const WORD LaseEdlAcquisitionThread::CMD_SET_ZONE = 0x0206; 00095 const WORD LaseEdlAcquisitionThread::CMD_GET_ZONE = 0x0207; 00096 const WORD LaseEdlAcquisitionThread::CMD_RELEASE_ZONE = 0x0208; 00097 const WORD LaseEdlAcquisitionThread::CMD_SET_FILTER = 0x0209; 00098 const WORD LaseEdlAcquisitionThread::CMD_SET_FUNCTION = 0x020A; 00099 const WORD LaseEdlAcquisitionThread::CMD_GET_FUNCTION = 0x020B; 00100 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MEASUREMENT = 0x0300; 00101 const WORD LaseEdlAcquisitionThread::CMD_GET_PROFILE = 0x0301; 00102 const WORD LaseEdlAcquisitionThread::CMD_CANCEL_PROFILE = 0x0302; 00103 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_WORKING = 0x0400; 00104 const WORD LaseEdlAcquisitionThread::CMD_DO_RESET = 0x0401; 00105 const WORD LaseEdlAcquisitionThread::CMD_TRANS_IDLE = 0x0402; 00106 const WORD LaseEdlAcquisitionThread::CMD_TRANS_ROTATE = 0x0403; 00107 const WORD LaseEdlAcquisitionThread::CMD_TRANS_MEASURE = 0x0404; 00108 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MAINTENANCE = 0x0500; 00109 const WORD LaseEdlAcquisitionThread::CMD_DO_ADJUST = 0x0501; 00110 const WORD LaseEdlAcquisitionThread::CMD_DO_TEST = 0x0502; 00111 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_INTERFACE_ROUTING = 0x0600; 00112 const WORD LaseEdlAcquisitionThread::CMD_COM_ATTACH = 0x0601; 00113 const WORD LaseEdlAcquisitionThread::CMD_COM_DETACH = 0x0602; 00114 const WORD LaseEdlAcquisitionThread::CMD_COM_INIT = 0x0603; 00115 const WORD LaseEdlAcquisitionThread::CMD_COM_OUTPUT = 0x0604; 00116 const WORD LaseEdlAcquisitionThread::CMD_COM_DATA = 0x0605; 00117 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_FILE = 0x0700; 00118 const WORD LaseEdlAcquisitionThread::CMD_DIR = 0x0701; 00119 const WORD LaseEdlAcquisitionThread::CMD_SAVE = 0x0702; 00120 const WORD LaseEdlAcquisitionThread::CMD_LOAD = 0x0703; 00121 const WORD LaseEdlAcquisitionThread::CMD_DELETE = 0x0704; 00122 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_MONITOR = 0x0900; 00123 const WORD LaseEdlAcquisitionThread::CMD_MONITOR_ENABLE_LOG = 0x0801; 00124 const WORD LaseEdlAcquisitionThread::CMD_MONITOR_DISABLE_LOG = 0x0802; 00125 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_ADJUST = 0x7E00; 00126 const WORD LaseEdlAcquisitionThread::SERVICEGROUP_SPECIAL = 0x7F00; 00127 const WORD LaseEdlAcquisitionThread::CMD_SERVICE_FAILURE = 0x7F00; 00128 const WORD LaseEdlAcquisitionThread::RESPONSE_BIT = 0x8000; 00129 00130 00131 const float LaseEdlAcquisitionThread::DISTANCE_FACTOR = 256.00; 00132 00133 00134 /** @class LaseEdlAcquisitionThread "lase_edl_aqt.h" 00135 * Laser acqusition thread for Lase EDL L A laser scanner. 00136 * This thread fetches the data from the laser. 00137 * @author Tim Niemueller 00138 * @author Christian Fritz 00139 */ 00140 00141 00142 /** Constructor. 00143 * @param cfg_name short name of configuration group 00144 * @param cfg_prefix configuration path prefix 00145 */ 00146 LaseEdlAcquisitionThread::LaseEdlAcquisitionThread(std::string &cfg_name, 00147 std::string &cfg_prefix) 00148 : LaserAcquisitionThread("LaseEdlAcquisitionThread") 00149 { 00150 set_name("LaseEDL(%s)", cfg_name.c_str()); 00151 __pre_init_done = false; 00152 __cfg_name = cfg_name; 00153 __cfg_prefix = cfg_prefix; 00154 } 00155 00156 00157 void 00158 LaseEdlAcquisitionThread::pre_init(fawkes::Configuration *config, 00159 fawkes::Logger *logger) 00160 { 00161 if (__pre_init_done) return; 00162 00163 try { 00164 std::string canres = config->get_string((__cfg_prefix + "canonical_resolution").c_str()); 00165 if (canres == "low") { 00166 __cfg_rotation_freq = 20; 00167 __cfg_angle_step = 16; 00168 } else if (canres == "high") { 00169 __cfg_rotation_freq = 15; 00170 __cfg_angle_step = 8; 00171 } else { 00172 logger->log_error(name(), "Canonical resolution %s is invalid, must be 'low' " 00173 "or 'high', trying to read raw config data"); 00174 throw Exception(""); 00175 } 00176 logger->log_debug(name(), "Using canonical resolution %s, freq: %u, angle step: %u", 00177 canres.c_str(), __cfg_rotation_freq, __cfg_angle_step); 00178 } catch (Exception &e) { 00179 // exceptions thrown here will propagate 00180 __cfg_rotation_freq = config->get_uint((__cfg_prefix + "rotation_freq").c_str()); 00181 __cfg_angle_step = config->get_uint((__cfg_prefix + "angle_step").c_str()); 00182 } 00183 00184 try { 00185 __cfg_use_default = config->get_bool((__cfg_prefix + "use_default").c_str()); 00186 __cfg_set_default = config->get_bool((__cfg_prefix + "set_default").c_str()); 00187 __cfg_max_pulse_freq = config->get_uint((__cfg_prefix + "max_pulse_freq").c_str()); 00188 __cfg_profile_format = config->get_uint((__cfg_prefix + "profile_format").c_str()); 00189 __cfg_can_id = config->get_uint((__cfg_prefix + "can_id").c_str()); 00190 __cfg_can_id_resp = config->get_uint((__cfg_prefix + "can_id_resp").c_str()); 00191 __cfg_sensor_id = config->get_uint((__cfg_prefix + "sensor_id").c_str()); 00192 __cfg_sensor_id_resp = config->get_uint((__cfg_prefix + "sensor_id_resp").c_str()); 00193 __cfg_btr0btr1 = config->get_uint((__cfg_prefix + "btr0btr1").c_str()); 00194 __cfg_port = config->get_uint((__cfg_prefix + "port").c_str()); 00195 __cfg_irq = config->get_uint((__cfg_prefix + "irq").c_str()); 00196 __cfg_num_init_tries = config->get_uint((__cfg_prefix + "num_init_tries").c_str()); 00197 __cfg_mount_rotation = config->get_float((__cfg_prefix + "mount_rotation").c_str()); 00198 00199 __min_angle_step = calc_angle_step(__cfg_rotation_freq, __cfg_max_pulse_freq); 00200 if ( __cfg_angle_step < __min_angle_step ) { 00201 logger->log_warn(name(), "Configured angle step %u less than required minimum " 00202 "of %u, raising to minimum", __cfg_angle_step, __min_angle_step); 00203 __cfg_angle_step = __min_angle_step; 00204 } 00205 __number_of_values = 16 * 360 / __cfg_angle_step; 00206 00207 if ( (__number_of_values != 360) && (__number_of_values != 720) ) { 00208 throw Exception("At the moment only configurations with 360 or 720 " 00209 "laser beams are supported, but %u requested", __number_of_values); 00210 } 00211 00212 _distances_size = _echoes_size = __number_of_values; 00213 00214 std::string interface_type = config->get_string((__cfg_prefix + "interface_type").c_str()); 00215 if ( interface_type == "usb" ) { 00216 __cfg_interface_type = HW_USB; 00217 } else { 00218 throw Exception("Unknown interface type %s", interface_type.c_str()); 00219 } 00220 00221 } catch (Exception &e) { 00222 e.append("Could not read all required config values for %s", name()); 00223 throw; 00224 } 00225 00226 __pre_init_done = true; 00227 } 00228 00229 void 00230 LaseEdlAcquisitionThread::init() 00231 { 00232 pre_init(config, logger); 00233 00234 init_bus(); 00235 00236 for (unsigned int i = 1; i <= __cfg_num_init_tries; ++i) { 00237 00238 try { 00239 CANCEL_PROFILE(); 00240 } catch (Exception &e) { 00241 // ignored, happens often 00242 } 00243 00244 try { 00245 logger->log_debug("LaseEdlAcquisitionThread", "Resetting Laser"); 00246 DO_RESET(RESETLEVEL_HALT_IDLE); 00247 00248 if ( ! __cfg_use_default ) { 00249 logger->log_debug("LaseEdlAcquisitionThread", "Setting configuration"); 00250 // set configuration (rotation and anglestep) 00251 SET_CONFIG(CONFIGITEM_GLOBAL, CONFIGDATA_LENGTH_GLOBAL, 00252 __cfg_sensor_id, __cfg_rotation_freq, __cfg_angle_step); 00253 00254 // set functions (sector definition) 00255 SET_FUNCTION(SECTOR_0, SECTORFUNC_NORMAL_MEASUREMENT, 00256 (16 * 360) - __cfg_angle_step, 00257 __cfg_set_default ? FLASH_YES : FLASH_NO); 00258 SET_FUNCTION(SECTOR_1, SECTORFUNC_NOT_INITIALIZED, 0, 00259 __cfg_set_default ? FLASH_YES : FLASH_NO); 00260 } 00261 00262 logger->log_debug("LaseEdlAcquisitionThread", "Starting rotating"); 00263 TRANS_ROTATE(__cfg_rotation_freq); 00264 logger->log_debug("LaseEdlAcquisitionThread", "Starting measuring"); 00265 TRANS_MEASURE(); 00266 logger->log_debug("LaseEdlAcquisitionThread", "Enable profile retrieval"); 00267 GET_PROFILE(PROFILENUM_CONTINUOUS, __cfg_profile_format); 00268 00269 break; // break for loop if initialization was successful 00270 } catch (Exception &e) { 00271 if (i < __cfg_num_init_tries) { 00272 logger->log_warn("LaseEdlAcquisitionThread", "Initialization, retrying %d more times", __cfg_num_init_tries - i); 00273 logger->log_warn("LaseEdlAcquisitionThread", e); 00274 } else { 00275 logger->log_error("LaseEdlAcquisitionThread", "Initialization failed, giving up after %u tries", __cfg_num_init_tries); 00276 throw; 00277 } 00278 } 00279 } 00280 00281 _distances = (float *)malloc(sizeof(float) * __number_of_values); 00282 _echoes = (float *)malloc(sizeof(float) * __number_of_values); 00283 } 00284 00285 00286 void 00287 LaseEdlAcquisitionThread::finalize() 00288 { 00289 free(_distances); 00290 free(_echoes); 00291 _distances = _echoes = NULL; 00292 00293 logger->log_debug("LaseEdlAcquisitionThread", "Resetting laser"); 00294 DO_RESET(RESETLEVEL_HALT_IDLE); 00295 } 00296 00297 00298 void 00299 LaseEdlAcquisitionThread::loop() 00300 { 00301 process_profiles(); 00302 } 00303 00304 00305 unsigned int 00306 LaseEdlAcquisitionThread::calc_angle_step(unsigned int rotation_freq, 00307 unsigned int max_pulse_freq) 00308 { 00309 float tmp; 00310 unsigned int rv; 00311 tmp = ( ((float)max_pulse_freq) / 360.0 ) / ((float)rotation_freq); 00312 tmp = ceil( (1 / tmp) * 16.0 ); 00313 rv = (unsigned int)tmp; 00314 00315 if (rv == 7 || rv == 11 || rv == 13 || rv == 14) rv++; 00316 00317 return rv; 00318 } 00319 00320 00321 void 00322 LaseEdlAcquisitionThread::init_bus() 00323 { 00324 FILE *f = fopen("/proc/pcan", "r"); 00325 if (! f) { 00326 throw Exception("Cannot open /proc/pcan, PCAN driver not loaded?"); 00327 } 00328 std::vector<std::string> config_lines; 00329 std::vector<std::string> device_lines; 00330 char tmp[128]; 00331 while (fgets(tmp, sizeof(tmp), f)) { 00332 if (tmp[0] == '*') { 00333 config_lines.push_back(tmp); 00334 } else if (tmp[0] != '\n') { 00335 device_lines.push_back(tmp); 00336 } 00337 } 00338 fclose(f); 00339 00340 std::vector<std::string>::iterator l; 00341 for (l = config_lines.begin(); l != config_lines.end(); ++l) { 00342 // proc is found 00343 std::string::size_type pos = 0; 00344 while ((pos = l->find("[", pos)) != std::string::npos) { 00345 pos += 1; 00346 std::string::size_type pos_end = l->find("]", pos); 00347 if (pos_end != std::string::npos) { 00348 std::string item = l->substr(pos, pos_end - pos); 00349 if (item == "net") { 00350 throw Exception("PCAN driver has been compiled in netdev mode, but " 00351 "chardev mode is required. Please read the plugin " 00352 "documentation and recompile the PCAN driver."); 00353 } 00354 } 00355 } 00356 } 00357 00358 __handle = CAN_Open(__cfg_interface_type, 0, __cfg_port, __cfg_irq); 00359 if (__handle == NULL) { 00360 throw Exception("Cannot open CAN bus"); 00361 } 00362 if (CAN_Init(__handle, __cfg_btr0btr1, CAN_INIT_TYPE_ST) != CAN_ERR_OK) { 00363 throw Exception("Cannot initialize CAN bus"); 00364 } 00365 } 00366 00367 00368 void 00369 LaseEdlAcquisitionThread::send(WORD *data, int n) 00370 { 00371 TPCANMsg msg; 00372 msg.ID = __cfg_can_id; 00373 msg.MSGTYPE = MSGTYPE_STANDARD; 00374 msg.LEN = 0; 00375 00376 int send_words = 0; 00377 WORD number_of_frames = 0; 00378 00379 // special case for less or equal two words 00380 if (n <= 2) { 00381 number_of_frames = 1; 00382 append_to_msg( (WORD)0, &msg); 00383 append_to_msg( (WORD)__cfg_sensor_id, &msg); 00384 if (n >= 1) { 00385 append_to_msg( data[0], &msg); 00386 } 00387 if (n == 2) { 00388 append_to_msg( data[1], &msg); 00389 } 00390 //printf("send (1): "); print_message(&msg); 00391 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) { 00392 throw Exception("Laser send() failed (1)"); 00393 } 00394 00395 } else { // more than 2 words 00396 number_of_frames = ((n - 1) / 3) + 1; 00397 if ((n-1) % 3 != 0) { 00398 ++number_of_frames; 00399 } 00400 append_to_msg( (WORD)0xFFFF, &msg); 00401 append_to_msg( number_of_frames, &msg); 00402 append_to_msg( (WORD)__cfg_sensor_id, &msg); 00403 append_to_msg( data[send_words++], &msg); 00404 // printf("send (2): "); print_message(&msg); 00405 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) { 00406 throw Exception("Laser send() failed (2)"); 00407 } 00408 00409 for (WORD f=number_of_frames-1; f > 1; --f ) { 00410 msg.LEN = 0; 00411 append_to_msg( f, &msg); 00412 append_to_msg( data[send_words++], &msg); 00413 append_to_msg( data[send_words++], &msg); 00414 append_to_msg( data[send_words++], &msg); 00415 // printf("send (3): "); print_message(&msg); 00416 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) { 00417 throw Exception("Laser send() failed (3)"); 00418 } 00419 } 00420 // last frame 00421 msg.LEN = 0; 00422 append_to_msg( (WORD)0x0001, &msg); 00423 for (int i=send_words; i < n; i++) { 00424 append_to_msg( data[send_words++], &msg); 00425 } 00426 // printf("send (4): "); print_message(&msg); 00427 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) { 00428 throw Exception("Laser send() failed (3)"); 00429 } 00430 } 00431 } 00432 00433 00434 int 00435 LaseEdlAcquisitionThread::recv(WORD **data, bool allocate) 00436 { 00437 TPCANMsg msg; 00438 // read from CAN BUS 00439 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) { 00440 throw Exception("Laser recv() failed (1)"); 00441 } 00442 // If msg wasn't send by our laser: ignore it 00443 if (msg.ID != __cfg_can_id_resp) { 00444 logger->log_warn("LaseEdlAcquisitionThread", "CAN ID is not the expected ID, " 00445 "ignoring message"); 00446 return -1; 00447 } 00448 00449 int number_of_incoming_frames = 0; 00450 WORD number_of_incoming_words = 0; 00451 int msg_index = 0; 00452 int data_index = 0; 00453 WORD read; 00454 00455 read = get_word_from_msg(&msg, &msg_index); 00456 00457 // seek for beginning of a block 00458 while ((read != 0x0000) && (read != 0xFFFF) ) { 00459 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) { 00460 throw Exception("Laser recv() failed (2)"); 00461 } 00462 msg_index = 0; 00463 read = get_word_from_msg( &msg, &msg_index); 00464 } 00465 00466 // got legal block: process it 00467 if (read == 0x0000) { // receiving only one frame 00468 read = get_word_from_msg( &msg, &msg_index); 00469 if (read != __cfg_sensor_id_resp) { 00470 logger->log_warn("LaseEdlAcquisitionThread", "Sensor ID is not the expected ID, " 00471 "ignoring message"); 00472 return -1; 00473 } 00474 number_of_incoming_words = (msg.LEN - msg_index) / 2; 00475 if (allocate) { 00476 (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_words)); 00477 } 00478 for (int i=0; i < number_of_incoming_words; ++i) { 00479 (*data)[i] = get_word_from_msg( &msg, &msg_index); 00480 } 00481 // printf("Received (1): "); print_word_array(number_of_incoming_words, *data); 00482 return number_of_incoming_words; 00483 } else if (read == 0xFFFF) { 00484 // get number of incoming frames 00485 number_of_incoming_frames = get_word_from_msg( &msg, &msg_index); 00486 if (allocate) { 00487 (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_frames * 6 + 1)); 00488 } 00489 data_index = 0; 00490 00491 // get sensor response ID 00492 read = get_word_from_msg( &msg, &msg_index); 00493 if (read != __cfg_sensor_id_resp) { 00494 logger->log_warn("LaseEdlAcquisitionThread", "Sensor ID is not the expected ID, " 00495 "ignoring message"); 00496 return -1; 00497 } 00498 00499 // two words remaining in first message 00500 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index); 00501 00502 // process all frames 00503 for (WORD f=number_of_incoming_frames-1; f > 0; --f ) { 00504 msg_index = 0; 00505 00506 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) { 00507 throw Exception("Laser recv() failed (3)"); 00508 } 00509 00510 // get and verify frame number indicator 00511 read = get_word_from_msg( &msg, &msg_index); 00512 if (read != f) { 00513 logger->log_warn("LaseEdlAcquisitionThread","Recv protocol violation, " 00514 "wrong frame number: expected %u, but got %u", f, read); 00515 return -1; 00516 } 00517 00518 // process all words in frame 00519 number_of_incoming_words = (msg.LEN - msg_index) >> 1; 00520 for (int i=0; i < number_of_incoming_words; ++i) { 00521 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index); 00522 } 00523 } 00524 00525 // printf("Received (2): "); print_word_array(data_index, *data); 00526 00527 // might be different from number_of_incoming_words, 00528 // since last message can be not full 00529 return data_index; 00530 00531 } else { 00532 logger->log_warn("LaseEdlAcquisitionThread", "Recv got strange first response word (neigther 0 nor FFFF)\n"); 00533 } 00534 return -1; 00535 } 00536 00537 00538 inline void 00539 LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg) 00540 { 00541 BYTE byte; 00542 byte = word >> 8; 00543 msg->DATA[(msg->LEN)++] = byte; 00544 byte = word; 00545 msg->DATA[(msg->LEN)++] = byte; 00546 } 00547 00548 00549 inline void 00550 LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg) 00551 { 00552 msg->DATA[(msg->LEN)++] = byte; 00553 } 00554 00555 inline WORD 00556 LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg, int *index) 00557 { 00558 WORD rv = msg->DATA[(*index)++] << 8; 00559 rv += msg->DATA[((*index)++)]; 00560 return rv; 00561 } 00562 00563 00564 WORD * 00565 LaseEdlAcquisitionThread::make_word_array(int count, ...) { 00566 va_list word_list; 00567 va_start(word_list, count); 00568 WORD *rtv; 00569 rtv = (WORD*)malloc( sizeof(WORD) * count); 00570 for (int i=0; i<count; ++i) { 00571 rtv[i] = (WORD) va_arg(word_list, int); 00572 } 00573 va_end(word_list); 00574 return rtv; 00575 } 00576 00577 00578 int 00579 LaseEdlAcquisitionThread::compare_word_arrays(int count, WORD* a, WORD* b) 00580 { 00581 for (int i=0; i < count; ++i) { 00582 if (a[i] != b[i]) { 00583 return 0; 00584 } 00585 } 00586 return 1; 00587 } 00588 00589 00590 void 00591 LaseEdlAcquisitionThread::print_word_array(int count, WORD* a) 00592 { 00593 for (int i=0; i < count; ++i) { 00594 printf("%04x ", a[i]); 00595 } 00596 printf("\n"); 00597 } 00598 00599 00600 void 00601 LaseEdlAcquisitionThread::print_message(TPCANMsg *m) 00602 { 00603 int i; 00604 printf("%c %c 0x%08x %1d ", 00605 (m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm', 00606 (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's', 00607 m->ID, 00608 m->LEN); 00609 00610 for (i = 0; i < m->LEN; i++) { 00611 printf("0x%02x ", m->DATA[i]); 00612 } 00613 00614 printf("\n"); 00615 } 00616 00617 void 00618 LaseEdlAcquisitionThread::process_profiles() 00619 { 00620 WORD* real_response; 00621 WORD* expected_response = make_word_array( 2, respcode(CMD_GET_PROFILE), 00622 __cfg_profile_format); 00623 int response_size = recv(&real_response); 00624 if (response_size == -1) { 00625 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): recv() failed"); 00626 return; 00627 } 00628 00629 // wrong answer ? 00630 if (! compare_word_arrays( 2, real_response, expected_response )) { 00631 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): Invalid response received"); 00632 return; 00633 } 00634 // wrong number of values ? 00635 if ( (response_size - 3 != (int)__number_of_values) && 00636 (response_size - 3 != 2 * (int)__number_of_values) ) { 00637 logger->log_warn("LaseEdlAcquisitionThread", "number of received values " 00638 "doesn't match my expectations, recvd %d, expected %d", 00639 response_size - 3, __number_of_values); 00640 return; 00641 } 00642 00643 // extract data from response 00644 register float dist = 0; 00645 register int echo = 0; 00646 register int dist_index = (int)roundf(__cfg_mount_rotation * 16 / __cfg_angle_step); 00647 register int echo_index = dist_index; 00648 00649 _data_mutex->lock(); 00650 _new_data = true; 00651 00652 // see which data is requested 00653 if (__cfg_profile_format == PROFILEFORMAT_DISTANCE ) { 00654 // only distances 00655 for (int i=3; i < response_size; ++i ) { 00656 dist = ((float)real_response[i]) / DISTANCE_FACTOR; 00657 _distances[__number_of_values - dist_index] = dist; 00658 if (++dist_index >= (int)__number_of_values) dist_index = 0; 00659 } 00660 00661 } else if (__cfg_profile_format == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE) ) { 00662 // distances + echos 00663 for (int i=3; i < response_size; ++i) { 00664 dist = ((float)real_response[i]) / DISTANCE_FACTOR; 00665 _distances[__number_of_values - dist_index] = dist; 00666 if (++dist_index >= (int)__number_of_values) dist_index = 0; 00667 ++i; 00668 echo = real_response[i]; 00669 _echoes[__number_of_values - echo_index] = echo; 00670 if (++echo_index >= (int)__number_of_values) echo_index = 0; 00671 } 00672 00673 00674 } else if (__cfg_profile_format == PROFILEFORMAT_ECHO_AMPLITUDE ) { 00675 // only echos 00676 for (int i=3; i < response_size; ++i ) { 00677 echo = real_response[i]; 00678 _echoes[__number_of_values - echo_index] = echo; 00679 if (++echo_index >= (int)__number_of_values) echo_index = 0; 00680 } 00681 } 00682 00683 _data_mutex->unlock(); 00684 00685 free( real_response ); 00686 free( expected_response ); 00687 } 00688 00689 00690 void 00691 LaseEdlAcquisitionThread::send_and_check(WORD *command_data, int command_length, 00692 WORD *expected_response, int n, 00693 WORD **real_response, int *response_size) 00694 { 00695 bool keep_response = (real_response != NULL); 00696 WORD **response; 00697 WORD *local_response; 00698 if (keep_response) { 00699 response = real_response; 00700 } else { 00701 response = &local_response; 00702 } 00703 send(command_data, command_length); 00704 int response_s = recv(response); 00705 00706 if (response_s <= 0) { 00707 throw Exception("Did not receive data for command"); 00708 } 00709 00710 bool match = compare_word_arrays(n, *response, expected_response); 00711 00712 if ( ! match || ! keep_response ) { 00713 free(*response); 00714 } 00715 free(expected_response); 00716 free(command_data); 00717 00718 if ( ! match) { 00719 throw Exception("Response to query did not match expectation"); 00720 } 00721 00722 if ( response_size != NULL ) { 00723 *response_size = response_s; 00724 } 00725 } 00726 00727 void 00728 LaseEdlAcquisitionThread::SET_CONFIG( WORD config_item, int k, ...) 00729 { 00730 WORD *command; 00731 command = (WORD*)malloc( sizeof(WORD) * (2+k) ); 00732 command[0] = CMD_SET_CONFIG; 00733 command[1] = config_item; 00734 va_list word_list; 00735 va_start( word_list, k); 00736 for (int i=0; i<k; ++i) { 00737 command[i+2] = (WORD) va_arg( word_list, int); 00738 } 00739 va_end( word_list ); 00740 00741 send_and_check(command, 2+k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2); 00742 } 00743 00744 00745 void 00746 LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func, 00747 WORD sect_stop, WORD flash ) 00748 { 00749 WORD* command = make_word_array(5, CMD_SET_FUNCTION, sect_num, sect_func, 00750 sect_stop, flash); 00751 send_and_check(command, 5, make_word_array(2, respcode(CMD_SET_FUNCTION), sect_num), 2); 00752 } 00753 00754 00755 void 00756 LaseEdlAcquisitionThread::GET_PROFILE( WORD prof_num, WORD prof_format) 00757 { 00758 WORD* command = make_word_array(3, CMD_GET_PROFILE, prof_num, prof_format); 00759 send_and_check(command, 3, 00760 make_word_array(2, respcode(CMD_GET_PROFILE), prof_format), 2); 00761 } 00762 00763 00764 void 00765 LaseEdlAcquisitionThread::CANCEL_PROFILE() 00766 { 00767 send_and_check(make_word_array(1, CMD_CANCEL_PROFILE), 1, 00768 make_word_array( 1, respcode(CMD_CANCEL_PROFILE)), 1); 00769 } 00770 00771 00772 void 00773 LaseEdlAcquisitionThread::DO_RESET(WORD reset_level) 00774 { 00775 WORD* command = make_word_array( 2, CMD_DO_RESET, reset_level); 00776 send_and_check(command, 2, make_word_array(2, respcode(CMD_DO_RESET), reset_level), 2); 00777 } 00778 00779 00780 void 00781 LaseEdlAcquisitionThread::TRANS_IDLE() 00782 { 00783 WORD* command = make_word_array( 1, CMD_TRANS_IDLE); 00784 WORD* real_response; 00785 int response_size; 00786 00787 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size); 00788 00789 bool failed = (real_response[response_size-1] != 0x0001); 00790 free(real_response); 00791 if (failed) throw Exception("Failed to set trans idle"); 00792 } 00793 00794 00795 void 00796 LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency) 00797 { 00798 WORD* command = make_word_array( 2, CMD_TRANS_ROTATE, frequency); 00799 WORD* real_response; 00800 int response_size; 00801 send_and_check(command, 2, make_word_array( 1, respcode(CMD_TRANS_ROTATE)), 1, 00802 &real_response, &response_size); 00803 00804 bool failed = (real_response[response_size-1] != 0x0002); 00805 free(real_response); 00806 if ( failed ) throw Exception("Failed to set trans rotate"); 00807 } 00808 00809 00810 void 00811 LaseEdlAcquisitionThread::TRANS_MEASURE() 00812 { 00813 WORD* command = make_word_array( 1, CMD_TRANS_MEASURE); 00814 WORD* real_response; 00815 int response_size; 00816 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_MEASURE)), 00817 1, &real_response, &response_size); 00818 00819 bool failed = (real_response[response_size-2] != 0x0003) || 00820 (real_response[response_size-1] != 0x0000); 00821 unsigned int error_code = real_response[response_size-1]; 00822 free(real_response); 00823 if ( failed ) throw Exception("Failed set trans measure, error code %u", error_code); 00824 }