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 <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 }

Generated on Tue Feb 22 13:31:29 2011 for Fawkes API by  doxygen 1.4.7