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