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 LaseEdlAcquisitionThread::LaseEdlAcquisitionThread()
00143 : LaserAcquisitionThread("LaseEdlAcquisitionThread")
00144 {
00145 __pre_init_done = false;
00146 }
00147
00148
00149 void
00150 LaseEdlAcquisitionThread::pre_init(fawkes::Configuration *config,
00151 fawkes::Logger *logger)
00152 {
00153 if (__pre_init_done) return;
00154
00155 try {
00156 std::string canres = config->get_string("/hardware/laser/canonical_resolution");
00157 if (canres == "low") {
00158 __cfg_rotation_freq = 20;
00159 __cfg_angle_step = 16;
00160 } else if (canres == "high") {
00161 __cfg_rotation_freq = 15;
00162 __cfg_angle_step = 8;
00163 } else {
00164 logger->log_error(name(), "Canonical resolution %s is invalid, must be 'low' "
00165 "or 'high', trying to read raw config data");
00166 throw Exception("");
00167 }
00168 logger->log_debug(name(), "Using canonical resolution %s, freq: %u, angle step: %u",
00169 canres.c_str(), __cfg_rotation_freq, __cfg_angle_step);
00170 } catch (Exception &e) {
00171
00172 __cfg_rotation_freq = config->get_uint("/hardware/laser/rotation_freq");
00173 __cfg_angle_step = config->get_uint("/hardware/laser/angle_step");
00174 }
00175
00176 try {
00177 __cfg_use_default = config->get_bool("/hardware/laser/use_default");
00178 __cfg_set_default = config->get_bool("/hardware/laser/set_default");
00179 __cfg_max_pulse_freq = config->get_uint("/hardware/laser/max_pulse_freq");
00180 __cfg_profile_format = config->get_uint("/hardware/laser/profile_format");
00181 __cfg_can_id = config->get_uint("/hardware/laser/can_id");
00182 __cfg_can_id_resp = config->get_uint("/hardware/laser/can_id_resp");
00183 __cfg_sensor_id = config->get_uint("/hardware/laser/sensor_id");
00184 __cfg_sensor_id_resp = config->get_uint("/hardware/laser/sensor_id_resp");
00185 __cfg_btr0btr1 = config->get_uint("/hardware/laser/btr0btr1");
00186 __cfg_port = config->get_uint("/hardware/laser/port");
00187 __cfg_irq = config->get_uint("/hardware/laser/irq");
00188 __cfg_num_init_tries = config->get_uint("/hardware/laser/num_init_tries");
00189 __cfg_mount_rotation = config->get_float("/hardware/laser/mount_rotation");
00190
00191 __min_angle_step = calc_angle_step(__cfg_rotation_freq, __cfg_max_pulse_freq);
00192 if ( __cfg_angle_step < __min_angle_step ) {
00193 logger->log_warn(name(), "Configured angle step %u less than required minimum "
00194 "of %u, raising to minimum", __cfg_angle_step, __min_angle_step);
00195 __cfg_angle_step = __min_angle_step;
00196 }
00197 __number_of_values = 16 * 360 / __cfg_angle_step;
00198
00199 if ( (__number_of_values != 360) && (__number_of_values != 720) ) {
00200 throw Exception("At the moment only configurations with 360 or 720 "
00201 "laser beams are supported, but %u requested", __number_of_values);
00202 }
00203
00204 _distances_size = _echoes_size = __number_of_values;
00205
00206 std::string interface_type = config->get_string("/hardware/laser/interface_type");
00207 if ( interface_type == "usb" ) {
00208 __cfg_interface_type = HW_USB;
00209 } else {
00210 throw Exception("Unknown interface type %s", interface_type.c_str());
00211 }
00212
00213 } catch (Exception &e) {
00214 e.append("Could not read all required config values for %s", name());
00215 throw;
00216 }
00217
00218 __pre_init_done = true;
00219 }
00220
00221 void
00222 LaseEdlAcquisitionThread::init()
00223 {
00224 pre_init(config, logger);
00225
00226 init_bus();
00227
00228 for (unsigned int i = 1; i <= __cfg_num_init_tries; ++i) {
00229
00230 try {
00231 CANCEL_PROFILE();
00232 } catch (Exception &e) {
00233
00234 }
00235
00236 try {
00237 logger->log_debug("LaseEdlAcquisitionThread", "Resetting Laser");
00238 DO_RESET(RESETLEVEL_HALT_IDLE);
00239
00240 if ( ! __cfg_use_default ) {
00241 logger->log_debug("LaseEdlAcquisitionThread", "Setting configuration");
00242
00243 SET_CONFIG(CONFIGITEM_GLOBAL, CONFIGDATA_LENGTH_GLOBAL,
00244 __cfg_sensor_id, __cfg_rotation_freq, __cfg_angle_step);
00245
00246
00247 SET_FUNCTION(SECTOR_0, SECTORFUNC_NORMAL_MEASUREMENT,
00248 (16 * 360) - __cfg_angle_step,
00249 __cfg_set_default ? FLASH_YES : FLASH_NO);
00250 SET_FUNCTION(SECTOR_1, SECTORFUNC_NOT_INITIALIZED, 0,
00251 __cfg_set_default ? FLASH_YES : FLASH_NO);
00252 }
00253
00254 logger->log_debug("LaseEdlAcquisitionThread", "Starting rotating");
00255 TRANS_ROTATE(__cfg_rotation_freq);
00256 logger->log_debug("LaseEdlAcquisitionThread", "Starting measuring");
00257 TRANS_MEASURE();
00258 logger->log_debug("LaseEdlAcquisitionThread", "Enable profile retrieval");
00259 GET_PROFILE(PROFILENUM_CONTINUOUS, __cfg_profile_format);
00260
00261 break;
00262 } catch (Exception &e) {
00263 if (i < __cfg_num_init_tries) {
00264 logger->log_warn("LaseEdlAcquisitionThread", "Initialization, retrying %d more times", __cfg_num_init_tries - i);
00265 logger->log_warn("LaseEdlAcquisitionThread", e);
00266 } else {
00267 logger->log_error("LaseEdlAcquisitionThread", "Initialization failed, giving up after %u tries", __cfg_num_init_tries);
00268 throw;
00269 }
00270 }
00271 }
00272
00273 _distances = (float *)malloc(sizeof(float) * __number_of_values);
00274 _echoes = (float *)malloc(sizeof(float) * __number_of_values);
00275 }
00276
00277
00278 void
00279 LaseEdlAcquisitionThread::finalize()
00280 {
00281 free(_distances);
00282 free(_echoes);
00283 _distances = _echoes = NULL;
00284
00285 logger->log_debug("LaseEdlAcquisitionThread", "Resetting laser");
00286 DO_RESET(RESETLEVEL_HALT_IDLE);
00287 }
00288
00289
00290 void
00291 LaseEdlAcquisitionThread::loop()
00292 {
00293 process_profiles();
00294 }
00295
00296
00297 unsigned int
00298 LaseEdlAcquisitionThread::calc_angle_step(unsigned int rotation_freq,
00299 unsigned int max_pulse_freq)
00300 {
00301 float tmp;
00302 unsigned int rv;
00303 tmp = ( ((float)max_pulse_freq) / 360.0 ) / ((float)rotation_freq);
00304 tmp = ceil( (1 / tmp) * 16.0 );
00305 rv = (unsigned int)tmp;
00306
00307 if (rv == 7 || rv == 11 || rv == 13 || rv == 14) rv++;
00308
00309 return rv;
00310 }
00311
00312
00313 void
00314 LaseEdlAcquisitionThread::init_bus()
00315 {
00316 __handle = CAN_Open(__cfg_interface_type, 0, __cfg_port, __cfg_irq);
00317 if (__handle == NULL) {
00318 throw Exception("Cannot open CAN bus");
00319 }
00320 if (CAN_Init(__handle, __cfg_btr0btr1, CAN_INIT_TYPE_ST) != CAN_ERR_OK) {
00321 throw Exception("Cannot initialize CAN bus");
00322 }
00323 }
00324
00325
00326 void
00327 LaseEdlAcquisitionThread::send(WORD *data, int n)
00328 {
00329 TPCANMsg msg;
00330 msg.ID = __cfg_can_id;
00331 msg.MSGTYPE = MSGTYPE_STANDARD;
00332 msg.LEN = 0;
00333
00334 int send_words = 0;
00335 WORD number_of_frames = 0;
00336
00337
00338 if (n <= 2) {
00339 number_of_frames = 1;
00340 append_to_msg( (WORD)0, &msg);
00341 append_to_msg( (WORD)__cfg_sensor_id, &msg);
00342 if (n >= 1) {
00343 append_to_msg( data[0], &msg);
00344 }
00345 if (n == 2) {
00346 append_to_msg( data[1], &msg);
00347 }
00348
00349 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00350 throw Exception("Laser send() failed (1)");
00351 }
00352
00353 } else {
00354 number_of_frames = ((n - 1) / 3) + 1;
00355 if ((n-1) % 3 != 0) {
00356 ++number_of_frames;
00357 }
00358 append_to_msg( (WORD)0xFFFF, &msg);
00359 append_to_msg( number_of_frames, &msg);
00360 append_to_msg( (WORD)__cfg_sensor_id, &msg);
00361 append_to_msg( data[send_words++], &msg);
00362
00363 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00364 throw Exception("Laser send() failed (2)");
00365 }
00366
00367 for (WORD f=number_of_frames-1; f > 1; --f ) {
00368 msg.LEN = 0;
00369 append_to_msg( f, &msg);
00370 append_to_msg( data[send_words++], &msg);
00371 append_to_msg( data[send_words++], &msg);
00372 append_to_msg( data[send_words++], &msg);
00373
00374 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00375 throw Exception("Laser send() failed (3)");
00376 }
00377 }
00378
00379 msg.LEN = 0;
00380 append_to_msg( (WORD)0x0001, &msg);
00381 for (int i=send_words; i < n; i++) {
00382 append_to_msg( data[send_words++], &msg);
00383 }
00384
00385 if (CAN_Write( __handle, &msg ) != CAN_ERR_OK) {
00386 throw Exception("Laser send() failed (3)");
00387 }
00388 }
00389 }
00390
00391
00392 int
00393 LaseEdlAcquisitionThread::recv(WORD **data, bool allocate)
00394 {
00395 TPCANMsg msg;
00396
00397 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
00398 throw Exception("Laser recv() failed (1)");
00399 }
00400
00401 if (msg.ID != __cfg_can_id_resp) {
00402 logger->log_warn("LaseEdlAcquisitionThread", "CAN ID is not the expected ID, "
00403 "ignoring message");
00404 return -1;
00405 }
00406
00407 int number_of_incoming_frames = 0;
00408 WORD number_of_incoming_words = 0;
00409 int msg_index = 0;
00410 int data_index = 0;
00411 WORD read;
00412
00413 read = get_word_from_msg(&msg, &msg_index);
00414
00415
00416 while ((read != 0x0000) && (read != 0xFFFF) ) {
00417 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
00418 throw Exception("Laser recv() failed (2)");
00419 }
00420 msg_index = 0;
00421 read = get_word_from_msg( &msg, &msg_index);
00422 }
00423
00424
00425 if (read == 0x0000) {
00426 read = get_word_from_msg( &msg, &msg_index);
00427 if (read != __cfg_sensor_id_resp) {
00428 logger->log_warn("LaseEdlAcquisitionThread", "Sensor ID is not the expected ID, "
00429 "ignoring message");
00430 return -1;
00431 }
00432 number_of_incoming_words = (msg.LEN - msg_index) / 2;
00433 if (allocate) {
00434 (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_words));
00435 }
00436 for (int i=0; i < number_of_incoming_words; ++i) {
00437 (*data)[i] = get_word_from_msg( &msg, &msg_index);
00438 }
00439
00440 return number_of_incoming_words;
00441 } else if (read == 0xFFFF) {
00442
00443 number_of_incoming_frames = get_word_from_msg( &msg, &msg_index);
00444 if (allocate) {
00445 (*data) = (WORD*)malloc( sizeof(WORD)* (number_of_incoming_frames * 6 + 1));
00446 }
00447 data_index = 0;
00448
00449
00450 read = get_word_from_msg( &msg, &msg_index);
00451 if (read != __cfg_sensor_id_resp) {
00452 logger->log_warn("LaseEdlAcquisitionThread", "Sensor ID is not the expected ID, "
00453 "ignoring message");
00454 return -1;
00455 }
00456
00457
00458 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
00459
00460
00461 for (WORD f=number_of_incoming_frames-1; f > 0; --f ) {
00462 msg_index = 0;
00463
00464 if (CAN_Read( __handle, &msg) != CAN_ERR_OK) {
00465 throw Exception("Laser recv() failed (3)");
00466 }
00467
00468
00469 read = get_word_from_msg( &msg, &msg_index);
00470 if (read != f) {
00471 logger->log_warn("LaseEdlAcquisitionThread","Recv protocol violation, "
00472 "wrong frame number: expected %u, but got %u", f, read);
00473 return -1;
00474 }
00475
00476
00477 number_of_incoming_words = (msg.LEN - msg_index) >> 1;
00478 for (int i=0; i < number_of_incoming_words; ++i) {
00479 (*data)[data_index++] = get_word_from_msg( &msg, &msg_index);
00480 }
00481 }
00482
00483
00484
00485
00486
00487 return data_index;
00488
00489 } else {
00490 logger->log_warn("LaseEdlAcquisitionThread", "Recv got strange first response word (neigther 0 nor FFFF)\n");
00491 }
00492 return -1;
00493 }
00494
00495
00496 inline void
00497 LaseEdlAcquisitionThread::append_to_msg(WORD word, TPCANMsg *msg)
00498 {
00499 BYTE byte;
00500 byte = word >> 8;
00501 msg->DATA[(msg->LEN)++] = byte;
00502 byte = word;
00503 msg->DATA[(msg->LEN)++] = byte;
00504 }
00505
00506
00507 inline void
00508 LaseEdlAcquisitionThread::append_to_msg(BYTE byte, TPCANMsg *msg)
00509 {
00510 msg->DATA[(msg->LEN)++] = byte;
00511 }
00512
00513 inline WORD
00514 LaseEdlAcquisitionThread::get_word_from_msg(TPCANMsg *msg, int *index)
00515 {
00516 WORD rv = msg->DATA[(*index)++] << 8;
00517 rv += msg->DATA[((*index)++)];
00518 return rv;
00519 }
00520
00521
00522 WORD *
00523 LaseEdlAcquisitionThread::make_word_array(int count, ...) {
00524 va_list word_list;
00525 va_start(word_list, count);
00526 WORD *rtv;
00527 rtv = (WORD*)malloc( sizeof(WORD) * count);
00528 for (int i=0; i<count; ++i) {
00529 rtv[i] = (WORD) va_arg(word_list, int);
00530 }
00531 va_end(word_list);
00532 return rtv;
00533 }
00534
00535
00536 int
00537 LaseEdlAcquisitionThread::compare_word_arrays(int count, WORD* a, WORD* b)
00538 {
00539 for (int i=0; i < count; ++i) {
00540 if (a[i] != b[i]) {
00541 return 0;
00542 }
00543 }
00544 return 1;
00545 }
00546
00547
00548 void
00549 LaseEdlAcquisitionThread::print_word_array(int count, WORD* a)
00550 {
00551 for (int i=0; i < count; ++i) {
00552 printf("%04x ", a[i]);
00553 }
00554 printf("\n");
00555 }
00556
00557
00558 void
00559 LaseEdlAcquisitionThread::print_message(TPCANMsg *m)
00560 {
00561 int i;
00562 printf("%c %c 0x%08x %1d ",
00563 (m->MSGTYPE & MSGTYPE_RTR) ? 'r' : 'm',
00564 (m->MSGTYPE & MSGTYPE_EXTENDED) ? 'e' : 's',
00565 m->ID,
00566 m->LEN);
00567
00568 for (i = 0; i < m->LEN; i++) {
00569 printf("0x%02x ", m->DATA[i]);
00570 }
00571
00572 printf("\n");
00573 }
00574
00575 void
00576 LaseEdlAcquisitionThread::process_profiles()
00577 {
00578 WORD* real_response;
00579 WORD* expected_response = make_word_array( 2, respcode(CMD_GET_PROFILE),
00580 __cfg_profile_format);
00581 int response_size = recv(&real_response);
00582 if (response_size == -1) {
00583 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): recv() failed");
00584 return;
00585 }
00586
00587
00588 if (! compare_word_arrays( 2, real_response, expected_response )) {
00589 logger->log_warn("LaseEdlAcquisitionThread", "process_profiles(): Invalid response received");
00590 return;
00591 }
00592
00593 if ( (response_size - 3 != (int)__number_of_values) &&
00594 (response_size - 3 != 2 * (int)__number_of_values) ) {
00595 logger->log_warn("LaseEdlAcquisitionThread", "number of received values "
00596 "doesn't match my expectations, recvd %d, expected %d",
00597 response_size - 3, __number_of_values);
00598 return;
00599 }
00600
00601
00602 register float dist = 0;
00603 register int echo = 0;
00604 register int dist_index = (int)roundf(__cfg_mount_rotation * 16 / __cfg_angle_step);
00605 register int echo_index = dist_index;
00606
00607 _data_mutex->lock();
00608 _new_data = true;
00609
00610
00611 if (__cfg_profile_format == PROFILEFORMAT_DISTANCE ) {
00612
00613 for (int i=3; i < response_size; i++ ) {
00614 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
00615 _distances[dist_index++] = dist;
00616 if (dist_index >= (int)__number_of_values) dist_index = 0;
00617 }
00618
00619 } else if (__cfg_profile_format == (PROFILEFORMAT_DISTANCE | PROFILEFORMAT_ECHO_AMPLITUDE) ) {
00620
00621 for (int i=3; i < response_size; ) {
00622 dist = ((float)real_response[i]) / DISTANCE_FACTOR;
00623 _distances[dist_index++] = dist;
00624 if (dist_index >= (int)__number_of_values) dist_index = 0;
00625 i++;
00626 echo = real_response[i];
00627 _echoes[echo_index++] = echo;
00628 if (echo_index >= (int)__number_of_values) echo_index = 0;
00629 i++;
00630 }
00631
00632
00633 } else if (__cfg_profile_format == PROFILEFORMAT_ECHO_AMPLITUDE ) {
00634
00635 for (int i=3; i < response_size; i++ ) {
00636 echo = real_response[i];
00637 _echoes[echo_index++] = echo;
00638 if (echo_index >= (int)__number_of_values) echo_index = 0;
00639 }
00640 }
00641
00642 _data_mutex->unlock();
00643
00644 free( real_response );
00645 free( expected_response );
00646 }
00647
00648
00649 void
00650 LaseEdlAcquisitionThread::send_and_check(WORD *command_data, int command_length,
00651 WORD *expected_response, int n,
00652 WORD **real_response, int *response_size)
00653 {
00654 bool keep_response = (real_response != NULL);
00655 WORD **response;
00656 WORD *local_response;
00657 if (keep_response) {
00658 response = real_response;
00659 } else {
00660 response = &local_response;
00661 }
00662 send(command_data, command_length);
00663 int response_s = recv(response);
00664
00665 if (response_s <= 0) {
00666 throw Exception("Did not receive data for command");
00667 }
00668
00669 bool match = compare_word_arrays(n, *response, expected_response);
00670
00671 if ( ! match || ! keep_response ) {
00672 free(*response);
00673 }
00674 free(expected_response);
00675 free(command_data);
00676
00677 if ( ! match) {
00678 throw Exception("Response to query did not match expectation");
00679 }
00680
00681 if ( response_size != NULL ) {
00682 *response_size = response_s;
00683 }
00684 }
00685
00686 void
00687 LaseEdlAcquisitionThread::SET_CONFIG( WORD config_item, int k, ...)
00688 {
00689 WORD *command;
00690 command = (WORD*)malloc( sizeof(WORD) * (2+k) );
00691 command[0] = CMD_SET_CONFIG;
00692 command[1] = config_item;
00693 va_list word_list;
00694 va_start( word_list, k);
00695 for (int i=0; i<k; ++i) {
00696 command[i+2] = (WORD) va_arg( word_list, int);
00697 }
00698 va_end( word_list );
00699
00700 send_and_check(command, 2+k, make_word_array(2, respcode(CMD_SET_CONFIG), 0x0000), 2);
00701 }
00702
00703
00704 void
00705 LaseEdlAcquisitionThread::SET_FUNCTION(WORD sect_num, WORD sect_func,
00706 WORD sect_stop, WORD flash )
00707 {
00708 WORD* command = make_word_array(5, CMD_SET_FUNCTION, sect_num, sect_func,
00709 sect_stop, flash);
00710 send_and_check(command, 5, make_word_array(2, respcode(CMD_SET_FUNCTION), sect_num), 2);
00711 }
00712
00713
00714 void
00715 LaseEdlAcquisitionThread::GET_PROFILE( WORD prof_num, WORD prof_format)
00716 {
00717 WORD* command = make_word_array(3, CMD_GET_PROFILE, prof_num, prof_format);
00718 send_and_check(command, 3,
00719 make_word_array(2, respcode(CMD_GET_PROFILE), prof_format), 2);
00720 }
00721
00722
00723 void
00724 LaseEdlAcquisitionThread::CANCEL_PROFILE()
00725 {
00726 send_and_check(make_word_array(1, CMD_CANCEL_PROFILE), 1,
00727 make_word_array( 1, respcode(CMD_CANCEL_PROFILE)), 1);
00728 }
00729
00730
00731 void
00732 LaseEdlAcquisitionThread::DO_RESET(WORD reset_level)
00733 {
00734 WORD* command = make_word_array( 2, CMD_DO_RESET, reset_level);
00735 send_and_check(command, 2, make_word_array(2, respcode(CMD_DO_RESET), reset_level), 2);
00736 }
00737
00738
00739 void
00740 LaseEdlAcquisitionThread::TRANS_IDLE()
00741 {
00742 WORD* command = make_word_array( 1, CMD_TRANS_IDLE);
00743 WORD* real_response;
00744 int response_size;
00745
00746 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_IDLE)), 1, &real_response, &response_size);
00747
00748 bool failed = (real_response[response_size-1] != 0x0001);
00749 free(real_response);
00750 if (failed) throw Exception("Failed to set trans idle");
00751 }
00752
00753
00754 void
00755 LaseEdlAcquisitionThread::TRANS_ROTATE(WORD frequency)
00756 {
00757 WORD* command = make_word_array( 2, CMD_TRANS_ROTATE, frequency);
00758 WORD* real_response;
00759 int response_size;
00760 send_and_check(command, 2, make_word_array( 1, respcode(CMD_TRANS_ROTATE)), 1,
00761 &real_response, &response_size);
00762
00763 bool failed = (real_response[response_size-1] != 0x0002);
00764 free(real_response);
00765 if ( failed ) throw Exception("Failed to set trans rotate");
00766 }
00767
00768
00769 void
00770 LaseEdlAcquisitionThread::TRANS_MEASURE()
00771 {
00772 WORD* command = make_word_array( 1, CMD_TRANS_MEASURE);
00773 WORD* real_response;
00774 int response_size;
00775 send_and_check(command, 1, make_word_array( 1, respcode(CMD_TRANS_MEASURE)),
00776 1, &real_response, &response_size);
00777
00778 bool failed = (real_response[response_size-2] != 0x0003) ||
00779 (real_response[response_size-1] != 0x0000);
00780 unsigned int error_code = real_response[response_size-1];
00781 free(real_response);
00782 if ( failed ) throw Exception("Failed set trans measure, error code %u", error_code);
00783 }
