Fawkes API  Fawkes Development Version
lase_edl_aqt.cpp
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 }