00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
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
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
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
00250 SET_CONFIG(CONFIGITEM_GLOBAL, CONFIGDATA_LENGTH_GLOBAL,
00251 __cfg_sensor_id, __cfg_rotation_freq, __cfg_angle_step);
00252
00253
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;
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
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
00356 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00357 throw Exception("Laser send() failed (1)");
00358 }
00359
00360 } else {
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
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
00381 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00382 throw Exception("Laser send() failed (3)");
00383 }
00384 }
00385
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
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
00404 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
00405 throw Exception("Laser recv() failed (1)");
00406 }
00407
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
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
00432 if (read == 0x0000) {
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
00447 return number_of_incoming_words;
00448 } else if (read == 0xFFFF) {
00449
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
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
00465 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
00466
00467
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
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
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
00491
00492
00493
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
00595 if (! compare_word_arrays( 2, real_response, expected_response )) {
00596 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): Invalid response received");
00597 return;
00598 }
00599
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
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
00618 if (__cfg_profile_format == PROFILEFORMAT_DISTANCE ) {
00619
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
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
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 }