[37] | 1 | /*********************************************************************
|
---|
| 2 | // created: 2014/02/02 - 12:08
|
---|
| 3 | // filename: SickLDMRSSensor.cpp
|
---|
| 4 | //
|
---|
| 5 | // author: Cyril Fougeray
|
---|
| 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
| 7 | //
|
---|
| 8 | // version: $Id: $
|
---|
| 9 | //
|
---|
| 10 | // purpose: The acquisition class of the SickLDMRS sensor
|
---|
| 11 | //
|
---|
| 12 | *********************************************************************/
|
---|
| 13 |
|
---|
| 14 | #include "SickLDMRSSensor.h"
|
---|
| 15 |
|
---|
| 16 | #include "SickSocket.h"
|
---|
| 17 | // #include "Pacpus/kernel/ComponentFactory.h"
|
---|
| 18 | #include "Pacpus/kernel/DbiteException.h"
|
---|
| 19 | #include "Pacpus/kernel/DbiteFileTypes.h"
|
---|
| 20 | #include "Pacpus/kernel/Log.h"
|
---|
| 21 | #include "Pacpus/PacpusTools/ShMem.h"
|
---|
| 22 |
|
---|
| 23 | #include <iostream>
|
---|
| 24 | #include <QTcpSocket>
|
---|
| 25 | #include <string>
|
---|
| 26 |
|
---|
| 27 | using namespace std;
|
---|
| 28 |
|
---|
| 29 | // #define SICKLDMRS_SH_MEM
|
---|
| 30 |
|
---|
| 31 | namespace pacpus {
|
---|
| 32 |
|
---|
| 33 | DECLARE_STATIC_LOGGER("pacpus.base.SickLDMRSSensor");
|
---|
| 34 |
|
---|
| 35 |
|
---|
| 36 | static const string kSickMemoryName = "SickLDMRS";
|
---|
| 37 |
|
---|
| 38 |
|
---|
| 39 | static const long MagicWord = 0xAFFEC0C2; // Sick LDMRS
|
---|
| 40 |
|
---|
| 41 |
|
---|
| 42 | static const int SICKLDMRS_SCANDATA_TYPE = 0x2202;
|
---|
| 43 | static const int SICKLDMRS_OBJECTDATA_TYPE = 0x2221;
|
---|
| 44 |
|
---|
| 45 | //////////////////////////////////////////////////////////////////////////
|
---|
| 46 | /// Constructor.
|
---|
| 47 | SickLDMRSSensor::SickLDMRSSensor(QObject *parent)
|
---|
| 48 | : ipaddr_("192.168.0.1"),
|
---|
| 49 | port_(12002),
|
---|
| 50 | name_("SickLDMRSDefault")
|
---|
| 51 | {
|
---|
| 52 | LOG_TRACE("constructor ("<< this->name_ << "), default settings");
|
---|
| 53 |
|
---|
| 54 | S_socket = new SickSocket(this);
|
---|
| 55 |
|
---|
| 56 | connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
|
---|
| 57 |
|
---|
| 58 | pendingBytes.time = 0;
|
---|
| 59 | pendingBytes.previousData = false;
|
---|
| 60 |
|
---|
| 61 | this->kSickDbtFileName = name_.toStdString()+".dbt";
|
---|
| 62 | this->kSickUtcFileName = name_.toStdString()+"_data.utc";
|
---|
| 63 | }
|
---|
| 64 |
|
---|
| 65 | SickLDMRSSensor::SickLDMRSSensor(QObject *parent, QString name, QString ip, int port, int recording)
|
---|
| 66 | : ipaddr_(ip),
|
---|
| 67 | port_(port),
|
---|
| 68 | name_(name),
|
---|
| 69 | recording_((bool)recording)
|
---|
| 70 | {
|
---|
| 71 |
|
---|
| 72 | LOG_TRACE("constructor ("<< this->name_ << "), configuration : " << this->ipaddr_<<":"<<this->port_);
|
---|
| 73 |
|
---|
| 74 | S_socket = new SickSocket(this);
|
---|
| 75 |
|
---|
| 76 | connect(S_socket, SIGNAL(configuration()), this, SLOT(configure()) );
|
---|
| 77 |
|
---|
| 78 | pendingBytes.time = 0;
|
---|
| 79 | pendingBytes.previousData = false;
|
---|
| 80 |
|
---|
| 81 | this->kSickDbtFileName = name_.toStdString()+".dbt";
|
---|
| 82 | this->kSickUtcFileName = name_.toStdString()+"_data.utc";
|
---|
| 83 | }
|
---|
| 84 |
|
---|
| 85 | //////////////////////////////////////////////////////////////////////////
|
---|
| 86 | /// Destructor.
|
---|
| 87 | SickLDMRSSensor::~SickLDMRSSensor()
|
---|
| 88 | {
|
---|
| 89 | LOG_TRACE("destructor ("<< this->name_ << ")");
|
---|
| 90 | delete S_socket;
|
---|
| 91 | }
|
---|
| 92 |
|
---|
| 93 | //////////////////////////////////////////////////////////////////////////
|
---|
| 94 | /// Called by the ComponentManager to start the component
|
---|
| 95 | void SickLDMRSSensor::startActivity()
|
---|
| 96 | {
|
---|
| 97 |
|
---|
| 98 | LOG_TRACE("Start Activity ("<< this->name_ <<")");
|
---|
| 99 |
|
---|
| 100 | S_socket->connectToServer(ipaddr_, port_);
|
---|
| 101 |
|
---|
| 102 | if (recording_) {
|
---|
| 103 | LOG_TRACE("(Recording) Starting recording...");
|
---|
| 104 |
|
---|
| 105 | try {
|
---|
| 106 | dbtFile_.open(kSickDbtFileName, WriteMode, TELEM_SICK, sizeof(SickLDMRS_dbt));
|
---|
| 107 | } catch (DbiteException & e) {
|
---|
| 108 | cerr << "error opening dbt file: "<< kSickDbtFileName << ", " << e.what() << endl;
|
---|
| 109 | return;
|
---|
| 110 | }
|
---|
| 111 |
|
---|
| 112 | // FIXME: use ofstream
|
---|
| 113 | // open the file with C function to be sure that it will exist
|
---|
| 114 | FILE * stream;
|
---|
| 115 | if (NULL == (stream = fopen(kSickUtcFileName.c_str(), "a+"))) {
|
---|
| 116 | LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
|
---|
| 117 | ::exit(-1);
|
---|
| 118 | } else {
|
---|
| 119 | fclose(stream);
|
---|
| 120 | }
|
---|
| 121 |
|
---|
| 122 | dataFile_.open(kSickUtcFileName.c_str(), ios_base::out|ios_base::binary|ios_base::app);
|
---|
| 123 | if (!dataFile_) {
|
---|
| 124 | LOG_FATAL("cannot open file '" << kSickUtcFileName.c_str() << "'");
|
---|
| 125 | ::exit(-1);
|
---|
| 126 | }
|
---|
| 127 | }
|
---|
| 128 | else
|
---|
| 129 | LOG_TRACE("(Recording) Not recording...");
|
---|
| 130 |
|
---|
| 131 |
|
---|
| 132 | #ifdef SICKLDMRS_SH_MEM
|
---|
| 133 | shmem_ = new ShMem(kSickMemoryName.c_str(), sizeof(ScanSickData));
|
---|
| 134 | #endif
|
---|
| 135 |
|
---|
| 136 | }
|
---|
| 137 |
|
---|
| 138 | //////////////////////////////////////////////////////////////////////////
|
---|
| 139 | /// Called by the ComponentManager to stop the component
|
---|
| 140 | void SickLDMRSSensor::stopActivity()
|
---|
| 141 | {
|
---|
| 142 | LOG_TRACE("destructor (" << this->name_ << ")");
|
---|
| 143 |
|
---|
| 144 | S_socket->sendToServer(QString((u_int32_t)0x0021));
|
---|
| 145 |
|
---|
| 146 | S_socket->closeSocket();
|
---|
| 147 |
|
---|
| 148 | if (recording_) {
|
---|
| 149 | LOG_TRACE("(Recording) Recording stopped");
|
---|
| 150 | dbtFile_.close();
|
---|
| 151 | dataFile_.close();
|
---|
| 152 | }
|
---|
| 153 |
|
---|
| 154 | #ifdef SICKLDMRS_SH_MEM
|
---|
| 155 | delete shmem_;
|
---|
| 156 | #endif
|
---|
| 157 | // delete generator;
|
---|
| 158 |
|
---|
| 159 | }
|
---|
| 160 |
|
---|
| 161 |
|
---|
| 162 |
|
---|
| 163 | //////////////////////////////////////////////////////////////////////////
|
---|
| 164 | /// look for the position of the magic word in the packet.
|
---|
| 165 | /// Return it if found else return -1
|
---|
| 166 | u_int32_t SickLDMRSSensor::findMagicWord(const char * message, const unsigned length)
|
---|
| 167 | {
|
---|
| 168 | if (length < 4) {
|
---|
| 169 | return -1;
|
---|
| 170 | }
|
---|
| 171 |
|
---|
| 172 | unsigned long i = 0;
|
---|
| 173 | while(*((u_int32_t*)(message+i)) != 0xC2C0FEAF){ // BigE
|
---|
| 174 | if (i == length) {
|
---|
| 175 | return -1;
|
---|
| 176 | }
|
---|
| 177 | ++i;
|
---|
| 178 | }
|
---|
| 179 | return i;
|
---|
| 180 |
|
---|
| 181 | }
|
---|
| 182 |
|
---|
| 183 | //////////////////////////////////////////////////////////////////////////
|
---|
| 184 | /// Return the size of the message contained in the packet
|
---|
| 185 | /// You have to provide the position of the magic word in the packet
|
---|
| 186 | u_int32_t SickLDMRSSensor::getMessageSize(const char * message, const unsigned length, const long magicWordIndex)
|
---|
| 187 | {
|
---|
| 188 |
|
---|
| 189 | // we need at least 12 bytes
|
---|
| 190 | if (length < 12) {
|
---|
| 191 | return 0;
|
---|
| 192 | }
|
---|
| 193 | return ((*(message+magicWordIndex+11))&0x000000FF)
|
---|
| 194 | + ((*(message+magicWordIndex+10)<<8)&0x0000FF00)
|
---|
| 195 | + ((*(message+magicWordIndex+9)<<16)&0x00FF0000)
|
---|
| 196 | + ((*(message+magicWordIndex+8)<<24)&0xFF000000)
|
---|
| 197 | + 24 ;
|
---|
| 198 | }
|
---|
| 199 |
|
---|
| 200 | //////////////////////////////////////////////////////////////////////////
|
---|
| 201 | /// @param size the total size of the message including the 2 headers and the body
|
---|
| 202 | /// @param length the number of available bytes in the packet
|
---|
| 203 | bool SickLDMRSSensor::isMessageComplete(const unsigned length, const long size)
|
---|
| 204 | {
|
---|
| 205 | if (size <= length) {
|
---|
| 206 | return true;
|
---|
| 207 | }
|
---|
| 208 | return false;
|
---|
| 209 | }
|
---|
| 210 |
|
---|
| 211 |
|
---|
| 212 | void SickLDMRSSensor::fillScanHeader( MessageLDMRS &msg )
|
---|
| 213 | {
|
---|
| 214 | // address = base + Data header size (24-byte long) + offset
|
---|
| 215 | msg.hScan.scanNumber = *((u_int16_t*)(msg.body+24));
|
---|
| 216 | msg.hScan.scannerStatus = *((u_int16_t*)(msg.body+24+2));
|
---|
| 217 | msg.hScan.phaseOffset = *((u_int16_t*)(msg.body+24+4));
|
---|
| 218 | msg.hScan.startNtpTime = *((u_int64_t*)(msg.body+24+6));
|
---|
| 219 | msg.hScan.endNtpTime = *((u_int64_t*)(msg.body+24+14));
|
---|
| 220 | msg.hScan.ticksPerRot= *((u_int16_t*)(msg.body+24+22)); // needed to compute angle (°)
|
---|
| 221 | msg.hScan.startAngle = *((int16_t*)(msg.body+24+24));
|
---|
| 222 | msg.hScan.endAngle = *((int16_t*)(msg.body+24+26));
|
---|
| 223 | msg.hScan.numPoints = *((u_int16_t*)(msg.body+24+28));
|
---|
| 224 |
|
---|
| 225 | // msg.hScan.mountingYawAngle = *((int16_t*)(msg.body+24+30));
|
---|
| 226 | // msg.hScan.mountingPitchAngle = *((int16_t*)(msg.body+24+32));
|
---|
| 227 | // msg.hScan.mountingRollAngle = *((int16_t*)(msg.body+24+34));
|
---|
| 228 | // msg.hScan.mountingX = *((int16_t*)(msg.body+24+36));
|
---|
| 229 | // msg.hScan.mountingY = *((int16_t*)(msg.body+24+38));
|
---|
| 230 | // msg.hScan.mountingZ = *((int16_t*)(msg.body+24+40));
|
---|
| 231 |
|
---|
| 232 |
|
---|
| 233 | LOG_TRACE("(hScan) Scan header data parsed, scan number " << msg.hScan.scanNumber);
|
---|
| 234 | LOG_TRACE("(hScan) Number of scanned points : " << msg.hScan.numPoints);
|
---|
| 235 | }
|
---|
| 236 |
|
---|
| 237 |
|
---|
| 238 | void SickLDMRSSensor::fillDataHeader(MessageLDMRS &msg)
|
---|
| 239 | {
|
---|
| 240 |
|
---|
| 241 | msg.hData.magicWord = ((*(msg.body+3))&0x000000FF) +
|
---|
| 242 | ((*(msg.body+2)<<8)&0x0000FF00) +
|
---|
| 243 | ((*(msg.body+1)<<16)&0x00FF0000)+
|
---|
| 244 | ((*(msg.body)<<24)&0xFF000000);
|
---|
| 245 | LOG_TRACE("(hData) Magic word read "<<msg.hData.magicWord);
|
---|
| 246 | // TODO check if OK
|
---|
| 247 |
|
---|
| 248 | msg.hData.sizePreviousMessage = ((*(msg.body+7))&0x000000FF)+
|
---|
| 249 | ((*(msg.body+6)<<8)&0x0000FF00)+
|
---|
| 250 | ((*(msg.body+5)<<16)&0x00FF0000)+
|
---|
| 251 | ((*(msg.body+4)<<24)&0xFF000000);
|
---|
| 252 | LOG_TRACE("(hData) previous message size : "<<msg.hData.sizePreviousMessage);
|
---|
| 253 |
|
---|
| 254 | msg.hData.sizeCurrentMessage =((*(msg.body+11))&0x000000FF)+
|
---|
| 255 | ((*(msg.body+10)<<8)&0x0000FF00)+
|
---|
| 256 | ((*(msg.body+9)<<16)&0x00FF0000)+
|
---|
| 257 | ((*(msg.body+8)<<24)&0xFF000000);
|
---|
| 258 |
|
---|
| 259 | LOG_TRACE("(hData) current message size : "<<msg.hData.sizeCurrentMessage);
|
---|
| 260 |
|
---|
| 261 |
|
---|
| 262 | msg.hData.deviceId = *(msg.body+13);
|
---|
| 263 |
|
---|
| 264 | msg.hData.dataType = ((*(msg.body+15))&0x000000FF)+((*(msg.body+14)<<8)&0x0000FF00);
|
---|
| 265 | LOG_TRACE("(hData) Data type : " << msg.hData.dataType);
|
---|
| 266 |
|
---|
| 267 |
|
---|
| 268 | msg.hData.ntpTime = ((*(msg.body+15))&0x000000FF)+
|
---|
| 269 | ((*(msg.body+14)<<8)&0x0000FF00)+
|
---|
| 270 | ((*(msg.body+13)<<16)&0x00FF0000)+
|
---|
| 271 | ((*(msg.body+12)<<24)&0xFF000000)+
|
---|
| 272 | ((*(msg.body+11))&0x000000FF)+
|
---|
| 273 | ((*(msg.body+10)<<8)&0x0000FF00)+
|
---|
| 274 | ((*(msg.body+9)<<16)&0x00FF0000)+
|
---|
| 275 | ((*(msg.body+8)<<24)&0xFF000000);
|
---|
| 276 | }
|
---|
| 277 |
|
---|
| 278 | //////////////////////////////////////////////////////////////////////////
|
---|
| 279 | /// this function is called when no complete message has been found
|
---|
| 280 | /// we set the flag previousData to true for the next processing stage
|
---|
| 281 | /// and we store the timestamp of the bytes acquisition
|
---|
| 282 | void SickLDMRSSensor::storePendingBytes(road_time_t time)
|
---|
| 283 | {
|
---|
| 284 | if (!pendingBytes.previousData)
|
---|
| 285 | {
|
---|
| 286 | pendingBytes.time = time;
|
---|
| 287 | pendingBytes.previousData = true;
|
---|
| 288 | }
|
---|
| 289 | }
|
---|
| 290 |
|
---|
| 291 |
|
---|
| 292 | void SickLDMRSSensor:: splitPacket(const char * packet, const int length, road_time_t time)
|
---|
| 293 | {
|
---|
| 294 | long index = 0;
|
---|
| 295 | long msgSize = 0;
|
---|
| 296 | bool msgComplete = false;
|
---|
| 297 |
|
---|
| 298 | // we are working on the previous not decoded data + the actual incoming packet
|
---|
| 299 | pendingBytes.data.append(packet,length);
|
---|
| 300 | LOG_TRACE("(Packet reconstitution) Pending bytes : " << pendingBytes.data.size() );
|
---|
| 301 |
|
---|
| 302 |
|
---|
| 303 | while (pendingBytes.data.size() > 0)
|
---|
| 304 | {
|
---|
| 305 | // we are looking for the MagicWord
|
---|
| 306 | index = findMagicWord(pendingBytes.data.c_str() , pendingBytes.data.size() );
|
---|
| 307 | LOG_TRACE("(Packet reconstitution) MagicWord index : " << index );
|
---|
| 308 | if (index == -1)
|
---|
| 309 | {
|
---|
| 310 | storePendingBytes(time);
|
---|
| 311 | // exit the while loop
|
---|
| 312 | break;
|
---|
| 313 | }
|
---|
| 314 |
|
---|
| 315 |
|
---|
| 316 | // we are looking for the size of the message (scan data + scan points)
|
---|
| 317 | msgSize = getMessageSize(pendingBytes.data.c_str(), pendingBytes.data.size(), index);
|
---|
| 318 | if (msgSize == 0)
|
---|
| 319 | {
|
---|
| 320 | storePendingBytes(time);
|
---|
| 321 | // exit the while loop
|
---|
| 322 | break;
|
---|
| 323 | }
|
---|
| 324 |
|
---|
| 325 | LOG_TRACE("(Packet reconstitution) Message size (data header(24) + size of the message) : " << msgSize );
|
---|
| 326 |
|
---|
| 327 | // we are verifying if the message is complete
|
---|
| 328 | msgComplete = isMessageComplete( pendingBytes.data.size() , msgSize );
|
---|
| 329 | if (msgComplete == false)
|
---|
| 330 | {
|
---|
| 331 | storePendingBytes(time);
|
---|
| 332 | // exit the while loop
|
---|
| 333 | break;
|
---|
| 334 | }
|
---|
| 335 |
|
---|
| 336 | // we have a complete message available that we can add to the list
|
---|
| 337 | MessageLDMRS msg;
|
---|
| 338 |
|
---|
| 339 | // we copy the bytes in the body message
|
---|
| 340 | char* messageData = (char*)malloc(msgSize);
|
---|
| 341 | memcpy(messageData, pendingBytes.data.c_str() + index, msgSize);
|
---|
| 342 |
|
---|
| 343 | msg.body = messageData;
|
---|
| 344 |
|
---|
| 345 | // we set the timestamp of the message
|
---|
| 346 | if (pendingBytes.previousData)
|
---|
| 347 | {
|
---|
| 348 | // the timestamp is the one of the previous packet
|
---|
| 349 | msg.time = pendingBytes.time;
|
---|
| 350 | pendingBytes.previousData = false;
|
---|
| 351 | }
|
---|
| 352 | else
|
---|
| 353 | {
|
---|
| 354 | // the timestamp is the one of the actual received packet
|
---|
| 355 | msg.time = time;
|
---|
| 356 | }
|
---|
| 357 |
|
---|
| 358 | // we add the message to the list
|
---|
| 359 | msgList.push_back(msg);
|
---|
| 360 | // and we suppress the processed bytes of the pending data
|
---|
| 361 | pendingBytes.data.erase(0, msgSize);
|
---|
| 362 | }
|
---|
| 363 | }
|
---|
| 364 |
|
---|
| 365 |
|
---|
| 366 | unsigned long SickLDMRSSensor::processMessage(MessageLDMRS &msg)
|
---|
| 367 | {
|
---|
| 368 | fillDataHeader(msg);
|
---|
| 369 | if (SICKLDMRS_SCANDATA_TYPE == msg.hData.dataType) {
|
---|
| 370 | LOG_TRACE("(Process Message) Scan Data Type!");
|
---|
| 371 | fillScanHeader(msg);
|
---|
| 372 |
|
---|
| 373 | int index = 24 + 44; // data header + scan header
|
---|
| 374 | ScanPoint* scanPoints = (ScanPoint*) malloc(sizeof(ScanPoint) * msg.hScan.numPoints);
|
---|
| 375 | int position = 0;
|
---|
| 376 |
|
---|
| 377 | for (int i = 0; i < msg.hScan.numPoints; ++i) {
|
---|
| 378 | ((ScanPoint*)(scanPoints + position))->layerEcho = *((uchar*)(msg.body + index));
|
---|
| 379 | ((ScanPoint*)(scanPoints + position))->flags = *((uchar*)(msg.body + index + 1));
|
---|
| 380 | ((ScanPoint*)(scanPoints + position))->angle = *((u_int16_t*)(msg.body + index + 2));
|
---|
| 381 | ((ScanPoint*)(scanPoints + position))->distance = *((u_int16_t*)(msg.body + index + 4));
|
---|
| 382 | ((ScanPoint*)(scanPoints + position))->echoPulseWidth = *((u_int16_t*)(msg.body + index + 6));
|
---|
| 383 | index += 10;
|
---|
| 384 | position += sizeof(ScanPoint);
|
---|
| 385 | }
|
---|
| 386 |
|
---|
| 387 | // raw data (message) no longer needed, free memory and replace with structured data
|
---|
| 388 | free(msg.body);
|
---|
| 389 | msg.body = (char*) scanPoints;
|
---|
| 390 | }
|
---|
| 391 | else if (msg.hData.dataType == SICKLDMRS_OBJECTDATA_TYPE){
|
---|
| 392 | LOG_TRACE("(Process Message) Object Data Type!");
|
---|
| 393 |
|
---|
| 394 |
|
---|
| 395 |
|
---|
| 396 | // TODO
|
---|
| 397 |
|
---|
| 398 | // raw message no longer needed, free memory
|
---|
| 399 | free(msg.body);
|
---|
| 400 | // msg.body = (char*) scanObjects;
|
---|
| 401 | }
|
---|
| 402 | else // irrelevant data type
|
---|
| 403 | free(msg.body); // free raw data
|
---|
| 404 |
|
---|
| 405 | return msg.hData.dataType;
|
---|
| 406 | }
|
---|
| 407 |
|
---|
| 408 | //////////////////////////////////////////////////////////////////////////
|
---|
| 409 | /// write the data on the disk:
|
---|
| 410 | /// - complete the dbt file with msg.hData
|
---|
| 411 | /// - complete the binary data file with the alascaData.point
|
---|
| 412 | void SickLDMRSSensor::writeData(MessageLDMRS &msg)
|
---|
| 413 | {
|
---|
| 414 | // record the data in a dbt file. The dbt data is only the structure DataHeader
|
---|
| 415 | // scan data are recorded in the sickldmrs_data.utc file
|
---|
| 416 |
|
---|
| 417 | SickLDMRS_dbt entry;
|
---|
| 418 | entry.timeStartFromSensor = msg.hData.ntpTime;
|
---|
| 419 | entry.hScan = msg.hScan;
|
---|
| 420 | entry.dataPos = dataFile_.tellp(); // absolute position of pointer in UTC file
|
---|
| 421 | entry.time = msg.time;
|
---|
| 422 | entry.timerange = msg.timerange;
|
---|
| 423 |
|
---|
| 424 | // write DBT
|
---|
| 425 | try {
|
---|
| 426 | dbtFile_.writeRecord(msg.time, msg.timerange, (char *) &entry, sizeof(SickLDMRS_dbt));
|
---|
| 427 | } catch (DbiteException & e) {
|
---|
| 428 | cerr << "error writing data: " << e.what() << endl;
|
---|
| 429 | return;
|
---|
| 430 | }
|
---|
| 431 |
|
---|
| 432 | // record the scan data in a binary file sickldmrs_data.utc with "UTC\0" to separate the data
|
---|
| 433 | for (unsigned int i = 0 ; i < msg.hScan.numPoints ; ++i) {
|
---|
| 434 | dataFile_.write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
|
---|
| 435 | }
|
---|
| 436 | // add a magic word to delimit the block of data
|
---|
| 437 | int32_t utcMagicWord = UTC_MAGIC_WORD;
|
---|
| 438 | dataFile_.write(reinterpret_cast<char*>(&(utcMagicWord)), sizeof(int32_t));
|
---|
| 439 |
|
---|
| 440 | }
|
---|
| 441 |
|
---|
| 442 |
|
---|
| 443 |
|
---|
| 444 | ///
|
---|
| 445 | void SickLDMRSSensor::configure(){
|
---|
| 446 | // Start measuring
|
---|
| 447 | // S_socket->sendToServer(QString((u_int32_t)0x0020));
|
---|
| 448 |
|
---|
| 449 | // LOG_TRACE(this->name_ +" configured.");
|
---|
| 450 | }
|
---|
| 451 |
|
---|
| 452 |
|
---|
| 453 | //////////////////////////////////////////////////////////////////////////
|
---|
| 454 | /// Event call by the SickSocket class when new data has been arrived
|
---|
| 455 | /// on the network.
|
---|
| 456 | void SickLDMRSSensor::customEvent(QEvent * e)
|
---|
| 457 | {
|
---|
| 458 | SickFrame * frame = ((SickFrameEvent*)e)->frame;
|
---|
| 459 |
|
---|
| 460 | // we try to find some messages in the current packet + the pending bytes of the previous incoming data
|
---|
| 461 | splitPacket(frame->msg, frame->size, frame->time);
|
---|
| 462 |
|
---|
| 463 | // we delete the heap variable
|
---|
| 464 | delete frame;
|
---|
| 465 |
|
---|
| 466 | // we test if we have some messages to decode
|
---|
| 467 | while ( !msgList.empty() )
|
---|
| 468 | {
|
---|
| 469 | // get the first (the eldest) message and process it
|
---|
| 470 | MessageLDMRS msgToProcess = msgList.front();
|
---|
| 471 | unsigned long type = processMessage(msgToProcess);
|
---|
| 472 |
|
---|
| 473 | if (type == SICKLDMRS_SCANDATA_TYPE)
|
---|
| 474 | {
|
---|
| 475 | // setState(ComponentBase::MONITOR_OK);
|
---|
| 476 | // write data on the disk
|
---|
| 477 | if (recording_)
|
---|
| 478 | writeData(msgList.front());
|
---|
| 479 |
|
---|
| 480 | #ifdef SICKLDMRS_SH_MEM
|
---|
| 481 | /// push data in shared memory
|
---|
| 482 | // First the scan info
|
---|
| 483 | SickLDMRS_shMem toWrite;
|
---|
| 484 | toWrite.time = msgToProcess.time;
|
---|
| 485 | toWrite.timerange = msgToProcess.timerange;
|
---|
| 486 | toWrite.scanInfo = msgToProcess.hScan;
|
---|
| 487 | shmem_->write(toWrite, sizeof(SickLDMRS_shMem));
|
---|
| 488 |
|
---|
| 489 | // Then, the points
|
---|
| 490 | for (unsigned int i = 0 ; i < msgToProcess.hScan.numPoints ; ++i) {
|
---|
| 491 | shmem_->write((msg.body + i*sizeof(ScanPoint)), sizeof(ScanPoint));
|
---|
| 492 | }
|
---|
| 493 | #endif
|
---|
| 494 |
|
---|
| 495 | }
|
---|
| 496 | else if (type == SICKLDMRS_OBJECTDATA_TYPE)
|
---|
| 497 | {
|
---|
| 498 | // Handled via CAN bus ?
|
---|
| 499 | }
|
---|
| 500 |
|
---|
| 501 | // (malloced memory) ScanPoints[] or ScanObjects[] no longer needed
|
---|
| 502 | free(msgToProcess.body);
|
---|
| 503 |
|
---|
| 504 | // removes the processed item of the list
|
---|
| 505 | msgList.pop_front();
|
---|
| 506 | }
|
---|
| 507 | }
|
---|
| 508 |
|
---|
| 509 | } // namespace pacpus
|
---|