LdmrsSopasLayer.cpp
Go to the documentation of this file.
00001 //
00002 // LdmrsSopasLayer.cpp
00003 //
00004 //
00005 #include "LdmrsSopasLayer.hpp"
00006 #include "../datatypes/EvalCaseResult.hpp"
00007 #include "../tools/errorhandler.hpp"
00008 
00009 namespace devices
00010 {
00011         
00012 using namespace datatypes;
00013 
00014 //
00015 // ****************************** LdmrsSopasLayer ************************************* //
00016 //
00017 
00018 
00019 
00020 LdmrsSopasLayer::LdmrsSopasLayer(Manager* manager,
00021                                                                         const UINT8 deviceId,
00022                                                                         std::string ipAddress,
00023                                                                         UINT16 portNumber,
00024                                                                         bool weWantFieldData,
00025                                                                         bool weWantScanData,
00026                                                                         bool readOnlyMode)
00027 {
00028         m_beVerbose = false;
00029         
00030         m_manager = manager;
00031         m_deviceId = deviceId;
00032         m_ipAddress = ipAddress;
00033         m_portNumber = portNumber;
00034         m_weWantFieldData = weWantFieldData;
00035         m_weWantScanData = weWantScanData;
00036         m_readOnlyMode = readOnlyMode;
00037 }
00038 
00039 
00040 
00041 LdmrsSopasLayer::~LdmrsSopasLayer()
00042 {
00043 }
00044 
00045 
00046 
00047 bool LdmrsSopasLayer::init(Tcp::DisconnectFunction disconnectFunction, void* obj)
00048 {
00049         bool success = SopasBase::init(CoLa_B,
00050                                                                         m_ipAddress,
00051                                                                         m_portNumber,
00052                                                                         m_weWantScanData,
00053                                                                         m_weWantFieldData,
00054                                                                         m_readOnlyMode,
00055                                                                         disconnectFunction, obj);
00056 
00057         if (success == true)
00058         {
00059                 // Success
00060                 printInfoMessage("LdmrsSopasLayer::init: SopasBase was initialized successfully.", m_beVerbose);
00061         }
00062         else
00063         {
00064                 printError("LdmrsSopasLayer::init: Failed, aborting!");
00065                 return false;
00066         }
00067         
00068         //
00069         // Connect to the sensor
00070         //
00071         success = connect();
00072         if (success == true)
00073         {
00074                 // Success
00075                 printInfoMessage("LdmrsSopasLayer::init: Connected to scanner successfully.", m_beVerbose);
00076         }
00077         else
00078         {
00079                 printError("LdmrsSopasLayer::init: Failed to connect to scanner, aborting!");
00080                 return false;
00081         }
00082         
00083         // Read the field and eval case configuration, but only if we want field data
00084         if (m_weWantFieldData == true)
00085         {
00086                 // Field configuration
00087                 printInfoMessage("LdmrsSopasLayer::init: Reading current field configuration.", m_beVerbose);
00088                 success = action_readFields();
00089                 if (success == true)
00090                 {
00091                         printInfoMessage("LdmrsSopasLayer::init: Successfully read the field configuration (" +
00092                                                                 ::toString(m_fields.getFields().size()) + " fields).", m_beVerbose);
00093 
00094                         // Post the fields to the manager
00095                         Fields* f = new Fields;
00096                         *f = m_fields;
00097                         f->setSourceId(m_deviceId);
00098                         m_manager->setDeviceData(f);
00099                 }
00100                 else
00101                 {
00102                         // Fail
00103                         printError("LdmrsSopasLayer::init: Failed to read the field configuration, aborting!");
00104                         return false;
00105                 }
00106                 
00107                 // Eval cases
00108                 printInfoMessage("LdmrsSopasLayer::init: Reading current eval case configuration.", m_beVerbose);
00109                 success = action_readEvalCases();
00110                 if (success == true)
00111                 {
00112                         printInfoMessage("LdmrsSopasLayer::init: Successfully read the eval cases (" +
00113                                                                 ::toString(m_evalCases.getEvalCases().size()) + " cases).", m_beVerbose);
00114                         
00115                         // Post the eval cases to the manager
00116                         EvalCases* e = new EvalCases;
00117                         *e = m_evalCases;
00118                         e->setSourceId(m_deviceId);
00119                         m_manager->setDeviceData(e);
00120                 }
00121                 else
00122                 {
00123                         // Fail
00124                         printError("LdmrsSopasLayer::init: Failed to read the eval cases, aborting!");
00125                         return false;
00126                 }
00127         }
00128         else
00129         {
00130                 printInfoMessage("LdmrsSopasLayer::init: Skipping field configuration as we want no field data.", m_beVerbose);
00131         }
00132         
00133         return success;
00134 }
00135 
00136 //
00137 // Register for the wanted events.
00138 //
00139 bool LdmrsSopasLayer::run()
00140 {
00141         bool beVerboseHere = m_beVerbose;
00142 //      beVerboseHere = true;
00143         
00144         printInfoMessage("LdmrsSopasLayer::run: Called.", beVerboseHere);
00145         
00146         bool result = false;
00147         
00148         // Field data?
00149         if (m_weWantFieldData == true)
00150         {
00151                 printInfoMessage("LdmrsSopasLayer::run: Now subscribing to EvalCaseResults.", beVerboseHere);
00152                 result = action_subscribeEvalCaseResults();
00153                 if (result == false)
00154                 {
00155                         // Fail
00156                         printError("LdmrsSopasLayer::run: Failed to subscribe to EvalCaseResults, aborting.");
00157                         return false;
00158                 }
00159                 printInfoMessage("LdmrsSopasLayer::run: Subscription to EvalCaseResults was successful.", beVerboseHere);
00160         }
00161         
00162         // Here, we could also subscribe to scan data. However, scans are handeled more efficientely over the LUX
00163         // interface, so we ignore this part here.
00164         if (m_weWantScanData == true)
00165         {
00166 //              printWarning("LdmrsSopasLayer::run: Scan data reading via SOPAS / CoLa is not implemented, ignoring this setting.");
00167                 printInfoMessage("LdmrsSopasLayer::run: Now subscribing to scan data.", beVerboseHere);
00168                 result = action_subscribeScanData();
00169                 if (result == false)
00170                 {
00171                         // Fail
00172                         printError("LdmrsSopasLayer::run: Failed to subscribe to scan data, aborting.");
00173                         return false;
00174                 }
00175                 printInfoMessage("LdmrsSopasLayer::run: Subscription to scan data was successful.", beVerboseHere);
00176                 
00177         }
00178         
00179         return true;
00180 }
00181 
00182 //
00183 // Save all field-related (=SOPAS) parameters permanently.
00184 // This function can be called from runlevel USER, so no login is required.
00185 //
00186 bool LdmrsSopasLayer::action_flashFieldParameters()
00187 {
00188         bool beVerboseHere = m_beVerbose;
00189 //      beVerboseHere = true;
00190         
00191         printInfoMessage("LdmrsSopasLayer::action_flashFieldParameters: Called.", beVerboseHere);
00192 
00193         if (isConnected() == false)
00194         {
00195                 printWarning("LdmrsSopasLayer::action_flashFieldParameters: LD-MRS not connected - aborting.");
00196                 return false;
00197         }
00198 
00199         bool result = false;
00200         SopasAnswer* answer = NULL;
00201         UINT8 parameterBuffer[128];
00202         UINT16 parameterBufferLen = 0;
00203 
00204         result = invokeMethod(index_meth_FlashFieldParameters, parameterBuffer, parameterBufferLen, answer);
00205 
00206         if ((result == true) && (answer != NULL) && (answer->isValid() == true) && (answer->size() > 0))
00207         {
00208                 // Evaluate answer. Successful?  0 = false, 1 = true
00209                 BYTE* buffer = answer->getBuffer();
00210                 UINT8 success = buffer[0];      // ::memread_UINT8 (buffer);
00211                 if (success == 0)
00212                 {
00213                         // 0 = Not successful
00214                         m_isLoggedIn = false;
00215                         result = false;
00216                         printError("LdmrsSopasLayer::action_flashFieldParameters: FlashFieldPara command was not successful!");
00217                 }
00218                 else
00219                 {
00220                         // Success
00221                         m_isLoggedIn = true;
00222                         result = true;
00223                         printInfoMessage("LdmrsSopasLayer::action_flashFieldParameters: Field parameters flashed successfully.", beVerboseHere);
00224                 }
00225         }
00226 
00227         // free memory for answer
00228         if (answer != NULL)
00229         {
00230                 delete answer;
00231                 answer = NULL;
00232         }
00233         
00234         printInfoMessage("LdmrsSopasLayer::action_flashFieldParameters: All done, leaving.", beVerboseHere);
00235         return result;
00236 }
00237 
00238 //
00239 // Save all scan-related (=LUX/MRS) parameters permanently.
00240 // This function can be called from runlevel USER, so no login is required.
00241 //
00242 bool LdmrsSopasLayer::action_flashMrsParameters()
00243 {
00244         bool beVerboseHere = m_beVerbose;
00245 //      beVerboseHere = true;
00246         
00247         printInfoMessage("LdmrsSopasLayer::action_flashMrsParameters: Called.", beVerboseHere);
00248 
00249         if (isConnected() == false)
00250         {
00251                 printWarning("LdmrsSopasLayer::action_flashMrsParameters: LD-MRS not connected - aborting.");
00252                 return false;
00253         }
00254 
00255         bool result = false;
00256         SopasAnswer* answer = NULL;
00257         UINT8 parameterBuffer[128];
00258         UINT16 parameterBufferLen = 0;
00259 
00260         result = invokeMethod(index_meth_MthdFlashLUXParameters, parameterBuffer, parameterBufferLen, answer);
00261 
00262         if ((result == true) && (answer != NULL) && (answer->isValid() == true) && (answer->size() > 0))
00263         {
00264                 // Evaluate answer. Successful?  0 = false, 1 = true
00265                 BYTE* buffer = answer->getBuffer();
00266                 UINT8 success = buffer[0];      // ::memread_UINT8 (buffer);
00267                 if (success == 0)
00268                 {
00269                         // 0 = Not successful
00270                         m_isLoggedIn = false;
00271                         result = false;
00272                         printError("LdmrsSopasLayer::action_flashMrsParameters: FlashLUXPara command was not successful!");
00273                 }
00274                 else
00275                 {
00276                         // Success
00277                         m_isLoggedIn = true;
00278                         result = true;
00279                         printInfoMessage("LdmrsSopasLayer::action_flashMrsParameters: MRS parameters flashed successfully.", beVerboseHere);
00280                 }
00281         }
00282 
00283         // free memory for answer
00284         if (answer != NULL)
00285         {
00286                 delete answer;
00287         }
00288 
00289         printInfoMessage("LdmrsSopasLayer::action_flashMrsParameters: All done, leaving.", beVerboseHere);
00290         
00291         return result;
00292 }
00293 
00294 
00295 //
00296 // Log in as "Authorized client".
00297 //
00298 // This is required to set parameters.
00299 //
00300 bool LdmrsSopasLayer::action_login()
00301 {
00302         bool beVerboseHere = m_beVerbose;
00303 //      beVerboseHere = true;
00304         
00305         // cola-B ByIndex
00306         printInfoMessage("LdmrsSopasLayer::action_login: Trying to log in as authorized client.", beVerboseHere);
00307 
00308         bool result = false;
00309 
00310         if (isConnected() == true)
00311         {
00312                 const static UINT32 passwordHash_LDMRS = 0xF4724744; // Hash for "client"
00313                 SopasAnswer* answer = NULL;
00314 
00315                 INT8 level = 3; // 3 = AUTHORIZEDCLIENT
00316                 UINT8 parameterBuffer[128];
00317                 UINT16 parameterBufferLen = 0;
00318 
00319                 colab::addIntegerToBuffer<INT8>(parameterBuffer, parameterBufferLen, level);                            // user level
00320                 colab::addIntegerToBuffer<UINT32>(parameterBuffer, parameterBufferLen, passwordHash_LDMRS); // password hash
00321 
00322                 result = invokeMethod(index_meth_SetAccessMode, parameterBuffer, parameterBufferLen, answer);
00323 
00324                 if ((result == true) && (answer != NULL) && (answer->isValid() == true) && (answer->size() > 0))
00325                 {
00326                         // Evaluate answer. Login successful?  0 = false, 1 = true
00327                         BYTE* buffer = answer->getBuffer();
00328                         UINT8 success = buffer[0];      // ::memread_UINT8 (buffer);
00329                         if (success == 0)
00330                         {
00331                                 // 0 = Not successful
00332                                 m_isLoggedIn = false;
00333                                 result = false;
00334                                 printError("LdmrsSopasLayer::action_login: Login was not successful!");
00335                         }
00336                         else
00337                         {
00338                                 // Success
00339                                 m_isLoggedIn = true;
00340                                 result = true;
00341                                 printInfoMessage("LdmrsSopasLayer::action_login: Login was successful.", beVerboseHere);
00342                         }
00343                 }
00344 
00345                 // free memory for answer
00346                 if (answer != NULL)
00347                 {
00348                         delete answer;
00349                 }
00350         }
00351         else
00352         {
00353                 printWarning("LdmrsSopasLayer::action_login: LD-MRS not connected - cannot login.");
00354         }
00355 
00356         return result;
00357 }
00358 
00359 
00360 //
00361 // Logout.
00362 //
00363 bool LdmrsSopasLayer::action_logout()
00364 {
00365         // cola-B ByIndex
00366         bool beVerboseHere = m_beVerbose;
00367 //      beVerboseHere = true;
00368         
00369         // cola-B ByIndex
00370         printInfoMessage("LdmrsSopasLayer::action_logout: Logging out now.", beVerboseHere);
00371 
00372         bool result = false;
00373 
00374         if (isConnected() == true)
00375         {
00376                 const static UINT32 passwordHash_LDMRS = 0x00000000; // Not needed for LOGOUT
00377                 SopasAnswer* answer = NULL;
00378 
00379                 INT8 level = 1; // 1 = Logout, 3 = AUTHORIZEDCLIENT
00380                 UINT8 parameterBuffer[128];
00381                 UINT16 parameterBufferLen = 0;
00382 
00383                 colab::addIntegerToBuffer<INT8>(parameterBuffer, parameterBufferLen, level);                            // user level
00384                 colab::addIntegerToBuffer<UINT32>(parameterBuffer, parameterBufferLen, passwordHash_LDMRS); // password hash
00385 
00386                 result = invokeMethod(index_meth_SetAccessMode, parameterBuffer, parameterBufferLen, answer);
00387 
00388                 if ((result == true) && (answer != NULL) && (answer->isValid() == true) && (answer->size() > 0))
00389                 {
00390                         // Evaluate answer. Logout successful?  0 = false, 1 = true
00391                         BYTE* buffer = answer->getBuffer();
00392                         UINT8 success = buffer[0];      // ::memread_UINT8 (buffer);
00393                         if (success == 0)
00394                         {
00395                                 // 0 = Not successful
00396                                 result = false;
00397                                 printError("LdmrsSopasLayer::action_logout: Logout was not successful!");
00398                         }
00399                         else
00400                         {
00401                                 // Success
00402                                 m_isLoggedIn = false;
00403                                 result = true;
00404                                 printInfoMessage("LdmrsSopasLayer::action_logout: Logout was successful.", beVerboseHere);
00405                         }
00406                 }
00407 
00408                 // free memory for answer
00409                 if (answer != NULL)
00410                 {
00411                         delete answer;
00412                 }
00413         }
00414         else
00415         {
00416                 printWarning("LdmrsSopasLayer::action_logout: LD-MRS not connected - cannot log out, aborting.");
00417         }
00418 
00419         return result;
00420 }
00421 
00422 
00423 //
00424 // Subscribe to EvalCase results. After the subscription, the scanner will send EvalCaseResults whenever
00425 // there is a change in the EvalCase status.
00426 //
00427 bool LdmrsSopasLayer::action_subscribeEvalCaseResults()
00428 {
00429         // cola-B ByIndex
00430         bool result = false;
00431 
00432         if (isConnected() == false)
00433         {
00434                 printError("LdmrsSopasLayer::action_subscribeEvalCaseResults: LD-MRS not connected, aborting!");
00435                 return false;
00436         }
00437         
00438         result = registerEvent((UINT16)(index_event_aEvalCaseResult));
00439 
00440         if (result == true)
00441         {
00442                 m_fieldEventIsRegistered = true;
00443         }
00444         else
00445         {
00446                 printError("LdmrsSopasLayer::action_subscribeEvalCaseResults: Failed to subscribe, aborting!");
00447         }
00448 
00449         return result;
00450 }
00451 
00452 //
00453 // Un-subscribe from the EvalCase results.
00454 //
00455 bool LdmrsSopasLayer::action_unSubscribeEvalCaseResults()
00456 {
00457         // cola-B ByIndex
00458         bool result = unregisterEvent((UINT16)(index_event_aEvalCaseResult));
00459 
00460         if (result == true)
00461         {
00462                 m_fieldEventIsRegistered = false;
00463         }
00464         else
00465         {
00466                 printError("LdmrsSopasLayer::action_unSubscribeEvalCases: Failed to un-subscribe, aborting!");
00467         }
00468         
00469         return result;
00470 }
00471 
00472 
00473 
00474 //
00475 // Subscribe to scan data. After the subscription, the scanner will send new scan data whenever
00476 // a scan is ready.
00477 //
00478 bool LdmrsSopasLayer::action_subscribeScanData()
00479 {
00480         bool beVerboseHere = m_beVerbose;
00481 //      beVerboseHere = true;
00482         printInfoMessage("LdmrsSopasLayer::action_subscribeScanData: Called.", beVerboseHere);
00483         
00484         // cola-B ByIndex
00485         bool result = false;
00486 
00487         if (isConnected() == false)
00488         {
00489                 printError("LdmrsSopasLayer::action_subscribeScanData: LD-MRS not connected, aborting!");
00490                 return false;
00491         }
00492         
00493         result = registerEvent((UINT16)(index_event_ScanDataMonitor));
00494 
00495         if (result == true)
00496         {
00497                 m_scanEventIsRegistered = true;
00498         }
00499         else
00500         {
00501                 printError("LdmrsSopasLayer::action_subscribeScanData: Failed to subscribe, aborting!");
00502         }
00503 
00504         printInfoMessage("LdmrsSopasLayer::action_subscribeScanData: Done, leaving.", beVerboseHere);
00505         return result;
00506 }
00507 
00508 
00509 //
00510 // Un-subscribe from the scan data.
00511 //
00512 bool LdmrsSopasLayer::action_unSubscribeScanData()
00513 {
00514         // cola-B ByIndex
00515         bool result = unregisterEvent((UINT16)(index_event_ScanDataMonitor));
00516 
00517         if (result == true)
00518         {
00519                 m_scanEventIsRegistered = false;
00520         }
00521         else
00522         {
00523                 printError("LdmrsSopasLayer::action_unSubscribeScanData: Failed to un-subscribe, aborting!");
00524         }
00525         
00526         return result;
00527 }
00528 
00529 
00530 
00531 
00532 bool LdmrsSopasLayer::action_readScanConfig()
00533 {
00534         SopasAnswer* answer = NULL;
00535         bool success = readVariable(index_var_ScanConfig, answer);
00536 
00537         if (success && answer != NULL && answer->size() > 0)
00538         {
00539                 // decode scanconfig
00540                 BYTE* bufferPos = answer->getBuffer();
00541                 ScanFreqEnum scanFreqEnum = ScanFreqEnum(memread_UINT8(bufferPos));
00542 
00543                 switch (scanFreqEnum)
00544                 {
00545                 case ScanFreq1250:
00546                         m_scanFreq = 12.5;
00547                         break;
00548                 case ScanFreq2500:
00549                         m_scanFreq = 25.;
00550                         break;
00551                 case ScanFreq5000:
00552                         m_scanFreq = 50.;
00553                         break;
00554                 }
00555 
00556                 UINT16 length = memread_UINT16(bufferPos);
00557 
00558                 if (length > 0)
00559                 {
00560                         m_angleResolution = angleToRad(memread_INT16(bufferPos));
00561                         m_scanStartAngle = angleToRad(memread_INT16(bufferPos));
00562                         m_scanEndAngle = angleToRad(memread_INT16(bufferPos));
00563                 }
00564         }
00565 
00566         if (answer != NULL)
00567         {
00568                 delete answer;
00569         }
00570 
00571         return success;
00572 }
00573 
00574 //
00575 // Read the current eval cases.
00576 // Note that this is 'just' the configuration of the eval cases, not their current evaluation status,
00577 // which is the EvalCaseResult.
00578 //
00579 bool LdmrsSopasLayer::action_readEvalCases()
00580 {
00581         SopasAnswer* answer = NULL;
00582         bool success = readVariable(index_var_evalCaseParam, answer);
00583 
00584         if (success)
00585         {
00586                 m_evalCases = colaB_evalCaseDecoder(answer);
00587         }
00588 
00589         if (answer != NULL)
00590         {
00591                 delete answer;
00592         }
00593 
00594         return success;
00595 }
00596 
00597 //
00598 // Decode the configured eval cases.
00599 //
00600 EvalCases LdmrsSopasLayer::colaB_evalCaseDecoder(SopasAnswer* answer)
00601 {
00602         bool beVerboseHere = m_beVerbose;
00603 //      beVerboseHere = true;
00604         
00605         printInfoMessage("LdmrsSopasLayer::colaB_evalCaseDecoder: Called.", beVerboseHere);
00606         
00607         EvalCases evalCases;
00608 
00609         if (answer != NULL && answer->size() > 0)
00610         {
00611                 // decode answer
00612                 BYTE* bufferPos = answer->getBuffer();
00613 
00614                 UINT16 arrayLength = memread_UINT16(bufferPos);
00615 
00616                 for (UINT16 i = 0; i < arrayLength; ++i)
00617                 {
00618                         EvalCase_ptr evalCasePtr(new EvalCase);
00619                         EvalCase& evalCase = *evalCasePtr; // just for easier access
00620 
00621                         evalCase.setVersionNumber(memread_UINT16(bufferPos));
00622                         evalCase.setCaseNumber(memread_UINT8(bufferPos));
00623                         evalCase.setStrategy(EvalCase::EvaluationStrategy(memread_UINT8(bufferPos)));
00624                         evalCase.setResultNegation(memread_UINT8(bufferPos) != 0);
00625                         evalCase.setResponseTime(memread_UINT32(bufferPos));
00626                         evalCase.setResponseTimeExtended(memread_UINT32(bufferPos));
00627                         evalCase.setOutputNumber(memread_UINT8(bufferPos));
00628                         // EnumX
00629                         // no hardware inputs available
00630 //                      evalCase.setHardwareInputs(EvalCaseParameter::inputState_from_UINT8(memread_UINT8(bufferPos), 2));
00631 //                      memread_UINT8(bufferPos); // reserved
00632                         UINT8 inputs = memread_UINT8(bufferPos);
00633                         evalCase.setLogicalInputState_from_UINT8(inputs);
00634                         memread_UINT8(bufferPos); // reserved
00635                         evalCase.setDistDependent(memread_UINT8(bufferPos) != 0);
00636                         evalCase.setMaxRadialCorridor(double(memread_UINT16(bufferPos)) / 1000. ); // conversion from [mm] to [m]
00637                         evalCase.setManipulationPrevention(EvalCase::ManipulationPrevention(memread_UINT8(bufferPos)));
00638                         evalCase.setBlankingSize(double(memread_UINT16(bufferPos)) / 1000.); // conversion from [mm] to [m]
00639                         evalCase.setMinFieldExp(double(memread_UINT16(bufferPos)) / 1000.); // conversion from [mm] to [m]
00640                         evalCase.setFieldNumber(memread_UINT8(bufferPos));
00641                         evalCase.setFilterType(EvalCase::FilterType(memread_UINT8(bufferPos)));
00642 
00643                         UINT16 nameLength = memread_UINT16(bufferPos);
00644 
00645                         std::string name = std::string((char *)bufferPos, nameLength);
00646                         bufferPos += nameLength;
00647                         evalCase.setCaseName(name);
00648                         printInfoMessage("LdmrsSopasLayer::colaB_evalCaseDecoder: Decoding EvalCase with the name " +
00649                                                                 name + ". Inputs: " + ::toHexString(inputs) + ".", beVerboseHere);
00650 
00651                         UINT16 commentLength = memread_UINT16(bufferPos);
00652                         std::string comment = std::string((char*)bufferPos, commentLength);
00653                         bufferPos += commentLength,
00654                                                  evalCase.setComment(comment);
00655 
00656                         evalCases.add(evalCasePtr);
00657                 }
00658         }
00659         
00660         printInfoMessage("LdmrsSopasLayer::colaB_evalCaseDecoder: Done. Decoded " +
00661                                                 ::toString(evalCases.getEvalCases().size()) + " cases.", beVerboseHere);
00662         
00663         return evalCases;
00664 }
00665 
00666 //
00667 // Encode the given eval cases.
00668 // Returns the number of bytes that have been written into the buffer.
00669 //
00670 UINT32 LdmrsSopasLayer::colaB_evalCaseEncoder(BYTE* buffer, const EvalCases& evalCases)
00671 {
00672         bool beVerboseHere = m_beVerbose;
00673 //      beVerboseHere = true;
00674         
00675         printInfoMessage("LdmrsSopasLayer::colaB_evalCaseEncoder: Called.", m_beVerbose);
00676         
00677         //
00678         // Encode the cases
00679         //
00680         BYTE* bufferPos = buffer;
00681 
00682         // Number of eval cases
00683         UINT16 arrayLength = evalCases.getEvalCases().size();
00684         memwrite_UINT16(bufferPos, arrayLength);
00685 
00686         // The cases
00687         for (UINT16 i = 0; i < arrayLength; i++)
00688         {
00689                 EvalCase* evalCase = evalCases.getEvalCases().at(i);
00690 
00691                 printInfoMessage("LdmrsSopasLayer::colaB_evalCaseEncoder: Encoding EvalCase with the name " +
00692                                                         evalCase->getCaseName() + ".", beVerboseHere);
00693                 
00694                 // Version number
00695                 UINT16 versionNumber = evalCase->getVersionNumber();
00696                 memwrite_UINT16(bufferPos, versionNumber);
00697         
00698                 // Case number
00699                 UINT8 caseNumber = evalCase->getCaseNumber();
00700                 memwrite_UINT8(bufferPos, caseNumber);
00701                 
00702                 // Eval strategy
00703                 UINT8 evalStrategy = (UINT8)(evalCase->getStrategy());
00704                 memwrite_UINT8(bufferPos, evalStrategy);
00705                 
00706                 // Result negation
00707                 bool resultNegation = evalCase->getResultNegation();
00708                 if (resultNegation == true)
00709                 {
00710                         memwrite_UINT8(bufferPos, 1);
00711                 }
00712                 else
00713                 {
00714                         memwrite_UINT8(bufferPos, 0);
00715                 }
00716 
00717                 // Response time
00718                 UINT32 responseTime = evalCase->getResponseTime();
00719                 memwrite_UINT32(bufferPos, responseTime);
00720 
00721                 // Response time extended
00722                 UINT32 responseTimeExtended = evalCase->getResponseTimeExtended();
00723                 memwrite_UINT32(bufferPos, responseTimeExtended);
00724 
00725                 // Output number
00726                 UINT8 outputNumber = evalCase->getOutputNumber();
00727                 memwrite_UINT8(bufferPos, outputNumber);
00728                 
00729                 // Assigned inputs
00730                 UINT8 logicalInputs = evalCase->getLogicalInputState_as_UINT8();
00731                 memwrite_UINT8(bufferPos, logicalInputs);
00732                 
00733                 memread_UINT8(bufferPos); // reserved
00734                 
00735                 // DistDependent
00736                 bool distDependent = evalCase->getDistDependent();
00737                 if (distDependent == true)
00738                 {
00739                         memwrite_UINT8(bufferPos, 1);
00740                 }
00741                 else
00742                 {
00743                         memwrite_UINT8(bufferPos, 0);
00744                 }
00745                 
00746                 // RadialCorridor
00747                 UINT16 maxRadialCorridor = (UINT16)((evalCase->getMaxRadialCorridor() * 1000.0) + 0.5);
00748                 memwrite_UINT16(bufferPos, maxRadialCorridor);
00749                 
00750                 // Sensitivity
00751                 UINT8 manipulationPrevention = (UINT8)(evalCase->getManipulationPrevention());  // Effectively boolean
00752                 memwrite_UINT8(bufferPos, manipulationPrevention);
00753                 
00754                 // Blanking size
00755                 UINT16 blankingSize = (UINT16)((evalCase->getBlankingSize() * 1000.0) + 0.5);
00756                 memwrite_UINT16(bufferPos, blankingSize);
00757                 
00758                 // MinFieldExp
00759                 UINT16 minFieldExp = (UINT16)((evalCase->getMinFieldExp() * 1000.0) + 0.5);
00760                 memwrite_UINT16(bufferPos, minFieldExp);
00761 
00762                 // Field number (of assigned field)
00763                 UINT8 fieldNumber = evalCase->getFieldNumber();
00764                 memwrite_UINT8(bufferPos, fieldNumber);
00765 
00766                 // Filter type
00767                 UINT8 filterType = (UINT8)(evalCase->getFilterType());
00768                 memwrite_UINT8(bufferPos, filterType);
00769 
00770                 // Name length + name
00771                 std::string name = evalCase->getCaseName();
00772                 memwrite_UINT16(bufferPos, name.length());
00773                 memwrite_string(bufferPos, name);
00774                 
00775                 // Comment length + comment
00776                 std::string comment = evalCase->getComment();
00777                 memwrite_UINT16(bufferPos, comment.length());
00778                 memwrite_string(bufferPos, comment);
00779         }
00780         
00781         // How many bytes have been used?
00782         UINT32 len = (UINT32)((UINT64)bufferPos - (UINT64)buffer);
00783         
00784         printInfoMessage("LdmrsSopasLayer::colaB_evalCaseEncoder: Done. Encoded " +
00785                                                 ::toString(evalCases.getEvalCases().size()) + " cases. Used " +
00786                                                 ::toString(len) + " bytes.", beVerboseHere);
00787         
00788         return len;
00789 }
00790 
00791 
00792 //
00793 // Write a single field.
00794 // fieldNum = Number of the field to be written (0..15).
00795 //
00796 // Must be logged in as AuthorizedClient in order to do this.
00797 //
00798 bool LdmrsSopasLayer::action_writeField(UINT16 fieldNum, const FieldParameter& para)
00799 {
00800         bool beVerboseHere = m_beVerbose;
00801 //      beVerboseHere = true;
00802         
00803         printInfoMessage("LdmrsSopasLayer::action_writeField: Called for field " + toString(fieldNum) + ".", beVerboseHere);
00804         bool result = false;
00805         
00806         if (fieldNum > 15)
00807         {
00808                 printError("LdmrsSopasLayer::action_writeField: FieldNum must be 0..15, but is " + toString(fieldNum) + ", aborting!");
00809                 return false;
00810         }
00811         
00812         // Build the internal data structure
00813         UINT32 bufferSize = 1024;
00814         UINT16 usedBufferLen = 0;
00815         BYTE* buffer = new BYTE[bufferSize];
00816         usedBufferLen = colaB_fieldEncoder(buffer, para);
00817         
00818         // Write the variable
00819         result = writeVariable(index_var_field000 + fieldNum, buffer, usedBufferLen);
00820 
00821         if (result == false)
00822         {
00823                 // Failure
00824                 printError("LdmrsSopasLayer::action_writeField: Failed to write field " + toString(fieldNum) + ", aborting!");
00825                 return false;
00826         }
00827         
00828         // All done
00829         printInfoMessage("LdmrsSopasLayer::action_writeField: All done, leaving.", beVerboseHere);
00830         return result;
00831 }
00832 
00833 
00834 //
00835 // Write all eval cases.
00836 //
00837 // Must be logged in as AuthorizedClient in order to do this.
00838 //
00839 bool LdmrsSopasLayer::action_writeEvalCases(const EvalCases& evalCases)
00840 {
00841         bool beVerboseHere = m_beVerbose;
00842 //      beVerboseHere = true;
00843         
00844         printInfoMessage("LdmrsSopasLayer::action_writeEvalCases: Called, with " + toString(evalCases.getEvalCases().size()) + " eval cases.", beVerboseHere);
00845         bool result = false;
00846         
00847         if (evalCases.getEvalCases().size() > 16)
00848         {
00849                 printError("LdmrsSopasLayer::action_writeEvalCases: The MRS can only handle up to 16 eval cases, aborting!");
00850                 return false;
00851         }
00852         
00853         // Build the internal data structure
00854         UINT32 bufferSize = 1024;
00855         UINT16 usedBufferLen = 0;
00856         BYTE* buffer = new BYTE[bufferSize];
00857         usedBufferLen = colaB_evalCaseEncoder(buffer, evalCases);
00858         
00859         // Write the variable
00860         result = writeVariable(index_var_evalCaseParam, buffer, usedBufferLen);
00861 
00862         if (result == false)
00863         {
00864                 // Failure
00865                 printError("LdmrsSopasLayer::action_writeEvalCases: Failed to write eval cases, aborting!");
00866                 return false;
00867         }
00868         
00869         // All done
00870         printInfoMessage("LdmrsSopasLayer::action_writeEvalCases: All done, leaving.", beVerboseHere);
00871         return result;
00872 }
00873 
00874 
00875 //
00876 // Read the current parameters of all fields.
00877 // The MRS always has 16 fields.
00878 //
00879 bool LdmrsSopasLayer::action_readFields()
00880 {
00881         SopasAnswer* answer = NULL;
00882         bool success;
00883         
00884         // Find out how many fields we've got
00885         // Unfortunately, this does not work with the MRS. The reply is
00886         // always "16".
00887 /*      success = readVariable(index_var_numOfParamFields, answer);
00888         UINT16 numOfParamFields = 0;
00889 
00890         if ((success == true) &&
00891                 (answer != NULL) &&
00892                 (answer->size() > 0))
00893         {
00894                 // always 0 !!! ???
00895                 BYTE* bufferPos = answer->getBuffer();
00896                 numOfParamFields = memread_UINT16(bufferPos);
00897 
00898                 printInfoMessage("LdmrsSopasLayer::action_readFields: NumOfParamFields=" + toString(numOfParamFields) + ".", m_beVerbose);
00899         }
00900 
00901         // Get rid of the answer
00902         if (answer != NULL)
00903         {
00904                 delete answer;
00905         }
00906 */
00907         // Now read all possible fields
00908         // As the parameter "index_var_numOfParamFields" seems unreliable, read all (!) possible fields.
00909         for (UINT16 i = 0; i < MAX_NUM_OF_FIELDS; ++i)
00910         {
00911                 printInfoMessage("LdmrsSopasLayer::action_readFields: Reading field " + toString(i) + ".", true);
00912                 success = readVariable(index_var_field000 + i, answer);
00913 
00914                 if ((success == true) &&
00915                         (answer != NULL) &&
00916                         (answer->size() > 0))
00917                 {
00918                         FieldParameter* fieldPtr = colaB_fieldDecoder(answer);
00919 
00920                         // handle dummy field answer: no field will be set
00921                         if (fieldPtr != NULL)
00922                         {
00923                                 m_fields.add(fieldPtr);
00924                         }
00925                 }
00926 
00927                 if (answer != NULL)
00928                 {
00929                         delete answer;
00930                         answer = NULL;
00931                 }
00932         }
00933 
00934         return success;
00935 }
00936 
00938 double LdmrsSopasLayer::angleToRad(INT32 angle)
00939 {
00940         return ((double)angle / 32.0) * deg2rad;
00941 }
00942 
00943 //
00944 // Decode an incoming Field strucure. This is the parameter set for one of the 16 possible
00945 // fields.
00946 //
00947 // Note: A field with field number = 0 is invalid.
00948 //
00949 FieldParameter* LdmrsSopasLayer::colaB_fieldDecoder(SopasAnswer* answer)
00950 {
00951         bool beVerboseHere = m_beVerbose;
00952 //      beVerboseHere = true;
00953         
00954         
00955         FieldParameter* fieldPtr(new FieldParameter);
00956 
00957         if ((answer != NULL) && (answer->size() > 0))
00958         {
00959                 FieldParameter& field = *fieldPtr; // just for easier access
00960                 BYTE* bufferPos = answer->getBuffer();
00961 
00962                 field.setDistScaleFactor(memread_float(bufferPos));             // [mm per step]
00963                 field.setDistScaleOffset(memread_float(bufferPos));             // [mm]
00964                 field.setAngleScaleFactor(memread_UINT32(bufferPos));   // 1/32°
00965                 field.setAngleScaleOffset(memread_INT32(bufferPos));    // 1/32°
00966                 field.setFieldTypeIntern(memread_UINT8(bufferPos));             // Field type (Rectangular or Segmented)
00967                 field.setFieldNumber(memread_UINT8(bufferPos));
00968 
00969                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: setDistScaleFactor: " + toString(field.getDistScaleFactor(), 2) + ".", beVerboseHere);
00970                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: setDistScaleOffset: " + toString(field.getDistScaleOffset(), 2) + ".", beVerboseHere);
00971                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: setAngleScaleFactor: " + toString(field.getAngleScaleFactor()) + ".", beVerboseHere);
00972                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: setAngleScaleOffset: " + toString(field.getAngleScaleOffset()) + ".", beVerboseHere);
00973                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: setFieldTypeIntern: " + toString((INT32)(field.getFieldTypeIntern())) + ".", beVerboseHere);
00974                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Field number: " + toString(field.getFieldNumber()) + ".", beVerboseHere);
00975 
00976                 // Segmented field
00977                 UINT16 fieldSeg = memread_UINT16(bufferPos);
00978                 if (fieldSeg == 1)
00979                 {
00980                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Found segmented field.", beVerboseHere);
00981 
00982                         UINT8 arrayLength = memread_UINT16(bufferPos);
00983                         FieldSegmented* seg(new FieldSegmented);
00984                         
00985                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: ArrayLength=" + toString(arrayLength) + ".", beVerboseHere);
00986 
00987                         // iterate through points to fill the polygon in the correct order
00988                         for (UINT8 i = 0; i < arrayLength; ++i)
00989                         {
00990                                 UINT16 angleIdx = memread_UINT16(bufferPos);
00991                                 UINT16 startDist = memread_UINT16(bufferPos);
00992                                 UINT16 endDist = memread_UINT16(bufferPos);
00993                                 
00994                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Point " + toString(i) + ": AngIdx=" +
00995                                                                 toString(angleIdx) + ", StartDist=" + toString(startDist) + ", EndDist=" +
00996                                                                 toString(endDist) + ".", beVerboseHere);
00997 
00998                                 // start dist not valid if 65535
00999                                 double s = (startDist == 65535) ? ::NaN_double : ((double)startDist / 1000.) * field.getDistScaleFactor() + (field.getDistScaleOffset() / 1000.); // from [mm] to [m]
01000                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: StartDist is " + toString(s, 2) + ".", beVerboseHere);
01001 
01002                                 // transform to 1/32° tics
01003                                 double angle = field.getAngleScaleOffset() + (double)angleIdx * field.getAngleScaleFactor();
01004                                 // Quick hack: The LD-MRS treats the angle direction differently as expected and defined in DIN70000. Therefore, the
01005                                 // sign has to be reversed here.
01006                                 angle = -angle;
01007                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Angle is " + toString(angle, 2) + ".", beVerboseHere);
01008 
01009                                 // end dist not valid if 65535
01010                                 double e = (endDist == 65535) ? ::NaN_double : ((double)endDist / 1000.) * field.getDistScaleFactor() + (field.getDistScaleOffset() / 1000.); // from [mm] to [m]
01011                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: EndDist is " + toString(e, 2) + ".", beVerboseHere);
01012 
01013                                 seg->addPoint(FieldSegmentedPoint(angleToRad(angle), s, e));
01014                         }
01015                         seg->computePolygon();
01016 
01017                         field.setField((FieldDescription*)(seg));
01018                 }
01019 
01020                 // Rectangular field
01021                 UINT16 fieldRect = memread_UINT16(bufferPos);
01022                 if (fieldRect == 1)
01023                 {
01024                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Found rectangular field.", beVerboseHere);
01025 
01026                         FieldRectangle* rect(new FieldRectangle);
01027                         INT32 refPointAngle = memread_INT32(bufferPos);
01028                         // Quick hack: The LD-MRS treats the angle direction differently as expected. Therefore, the
01029                         // sign has to be reversed here.
01030                         refPointAngle = -refPointAngle;
01031                         UINT32 refPointDist = (memread_UINT16(bufferPos) * field.getDistScaleFactor()) + field.getDistScaleOffset();
01032                         INT32 rotAngle = memread_INT32(bufferPos);
01033                         // Quick hack: The LD-MRS treats the angle direction differently as expected. Therefore, the
01034                         // sign has to be reversed here.
01035                         rotAngle = -rotAngle;
01036                         UINT32 length = memread_UINT32(bufferPos); // width and length changed due to other coordiante system in LD-MRS!!!
01037                         UINT32 width = memread_UINT32(bufferPos);
01038 
01039                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: refPointDist is " + toString(refPointDist) + ".", beVerboseHere);
01040                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: refPointAngle is " + toString(-refPointAngle) + ".", beVerboseHere);
01041                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: rotAngle is " + toString(rotAngle, 2) + ".", beVerboseHere);
01042                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: length is " + toString(length, 2) + ".", beVerboseHere);
01043                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: width is " + toString(width, 2) + ".", beVerboseHere);
01044 
01045                         // convert to [m] and [rad]
01046                         rect->setRefPointAngle(angleToRad(refPointAngle)); // 1/32°
01047                         rect->setRefPointDist((double)refPointDist / 1000.);
01048                         rect->setRotAngle(angleToRad(rotAngle));
01049                         rect->setWidth((double) width / 1000.);
01050                         rect->setLength((double) length / 1000.);
01051 
01052                         rect->computePolygon();
01053 
01054                         field.setField((FieldDescription*)(rect));
01055                 }
01056 
01057                 // Radial field
01058                 UINT16 fieldRad = memread_UINT16(bufferPos);
01059                 if (fieldRad == 1)
01060                 {
01061                         printError("LdmrsSopasLayer::colaB_fieldDecoder: Found radial field, but the LD-MRS does not support radial fields!");
01062                         return NULL;
01063                 }
01064 
01065                 // Dynamic field
01066                 UINT16 fieldDyn = memread_UINT16(bufferPos);
01067                 if (fieldDyn == 1)
01068                 {
01069                         printError("LdmrsSopasLayer::colaB_fieldDecoder: Found dynamic field, but the LD-MRS does not support dynamic fields!");
01070                         return NULL;
01071                 }
01072 
01073                 // The rest of the data
01074                 UINT16 versionNumber = memread_UINT16(bufferPos);
01075                 field.setVersionNumber(versionNumber);
01076                 printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Field version number= " + toString(versionNumber) + ".", beVerboseHere);
01077                 UINT16 fieldNameLength = memread_UINT16(bufferPos);
01078 
01079                 if (fieldNameLength > 0)
01080                 {
01081                         std::string fieldName = colab::getStringFromBuffer(bufferPos, fieldNameLength);
01082                         field.setFieldName(fieldName);
01083 
01084                         printInfoMessage("LdmrsSopasLayer::colaB_fieldDecoder: Field name= " + fieldName + ".", beVerboseHere);
01085                 }
01086 
01087                 UINT16 commentLength = memread_UINT16(bufferPos);
01088 
01089                 if (commentLength > 0)
01090                 {
01091                         std::string comment = colab::getStringFromBuffer(bufferPos, commentLength);
01092                         field.setComment(comment);
01093                 }
01094 
01095                 // Rest of information not needed and therefore not decoded
01096                 // - bool enable layer filter
01097                 // - layer bit field
01098         }
01099 
01100         return fieldPtr;
01101 }
01102 
01103 
01104 //
01105 // Decodes incoming scans.
01106 //
01107 // Note that the created scans are not sorted by (increasing) hporizontal scanpoint angle. Instead, the scanpoints
01108 // are decoded into the scan structure in the order in which they are transmitted in the message. If a sorted
01109 // scan is required, this has to be done.
01110 //
01111 void LdmrsSopasLayer::scanDataDecoder(SopasEventMessage& frame)
01112 {
01113         bool beVerboseHere = m_beVerbose;
01114         beVerboseHere = true;   // false;
01115         
01116         // cola-B by index
01117         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Called. Decoding SOPAS frame with length=" +
01118                                                 ::toString(frame.size()) + " bytes.", beVerboseHere);
01119 
01120         // Decode this data only if we want the results
01121         if (m_weWantScanData == false)
01122         {
01123                 // No scan data wanted, so ignore this message.
01124                 return;
01125         }
01126 
01127         std::string receivedName;
01128         UINT32 receiveIndex;
01129         BYTE* bufferPos;
01130 
01131         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Reading variable index.", beVerboseHere);
01132         receiveIndex = frame.getVariableIndex(); // byte number 12 13
01133 
01134         if (receiveIndex != index_event_ScanDataMonitor)
01135         {
01136                 printError("LdmrsSopasLayer::scanDataDecoder: This is not a scan data message, aborting!");
01137                 return;
01138         }
01139         else
01140         {
01141                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: This is a scan data message.", beVerboseHere);
01142         }
01143         
01144         // Obviously scan data events are registered. This is just for global information.
01145         m_scanEventIsRegistered = true;
01146 
01147         bufferPos = &(frame.getPayLoad()[5]); // pay load without "sSI + index (2 byte)"
01148         UINT16 versionNumber = memread_UINT16(bufferPos);
01149         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Version number=" + ::toString(versionNumber) + ".", beVerboseHere);
01150 
01151         // Device block
01152         UINT16 logicalNumber = memread_UINT16(bufferPos);
01153         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Logical number of the device=" + ::toString(logicalNumber) + ".", beVerboseHere);
01154         UINT32 serialNumber = memread_UINT32(bufferPos);
01155         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Serial number of the device=" + ::toString(serialNumber) + ".", beVerboseHere);
01156         UINT16 statusBits = memread_UINT16(bufferPos);
01157         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Status bits=" + ::toString(statusBits) + ".", beVerboseHere);
01158         
01159         // Status block
01160         UINT16 telegramCount = memread_UINT16(bufferPos);
01161         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Telegram count=" + ::toString(telegramCount) + ".", beVerboseHere);
01162         UINT16 scanCount = memread_UINT16(bufferPos);
01163         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scan count=" + ::toString(scanCount) + ".", beVerboseHere);
01164         UINT32 systemTimeScan = memread_UINT32(bufferPos);
01165         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: System time scan=" + ::toString(systemTimeScan) + ".", beVerboseHere);
01166         UINT32 systemTimeTransmit = memread_UINT32(bufferPos);
01167         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: System time transmit=" + ::toString(systemTimeTransmit) + ".", beVerboseHere);
01168         UINT16 inputState = memread_UINT16(bufferPos);
01169         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Input state=" + ::toString(inputState) + ".", beVerboseHere);
01170         UINT16 outputState = memread_UINT16(bufferPos);
01171         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Output state=" + ::toString(outputState) + ".", beVerboseHere);
01172         UINT16 reserved_1 = memread_UINT16(bufferPos);
01173         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Reserved_1=" + ::toString(reserved_1) + ".", beVerboseHere);
01174         UINT32 scanFrequency = memread_UINT32(bufferPos);       // [1/256 Hz]
01175         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scan frequency=" + ::doubleToString(double(scanFrequency) / 256.0, 2) + " Hz.", beVerboseHere);
01176         UINT32 measurementFrequency = memread_UINT32(bufferPos);        // [100 Hz]
01177         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Measurement frequency=" + ::doubleToString(double(measurementFrequency) / 10.0, 1) + " kHz.", beVerboseHere);
01178         
01179         // Encoder block (Always 0 as the MRS has no encoder inputs)
01180         UINT16 numOfEncoders = memread_UINT16(bufferPos);
01181         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of encoder blocks=" + ::toString(numOfEncoders) + ".", beVerboseHere);
01182         for (UINT16 i = 0; i<numOfEncoders; i++)
01183         {
01184                 UINT32 encoderPos = memread_UINT32(bufferPos);  // [inc]
01185                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Encoder position=" + ::toString(encoderPos) + " increments.", beVerboseHere);
01186                 INT16 encoderSpeed = memread_INT16(bufferPos);  // [inc/mm]
01187                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Encoder speed=" + ::toString(encoderSpeed) + " inc/mm.", beVerboseHere);
01188         }
01189         
01190         // 16-bit data channels
01191         UINT16 numOf16BitDataChannels = memread_UINT16(bufferPos);
01192         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of 16-bit data channels=" + ::toString(numOf16BitDataChannels) + ".", beVerboseHere);
01193         
01194         // Setup of the vertical layer angles. Only valid for standard "4-layer" devices,
01195         // so-called "8-layer" devices have different angles!
01196         double vAngles[4];
01197         vAngles[0] = -1.6 * 0.75 * deg2rad;
01198         vAngles[1] = -1.6 * 0.25 * deg2rad;
01199         vAngles[2] = 1.6 * 0.25 * deg2rad;
01200         vAngles[3] = 1.6 * 0.75 * deg2rad;
01201         
01202         // Create the new scan
01203         Scan* scan = new Scan;
01204         // Decode the range data
01205         for (UINT16 i = 0; i<numOf16BitDataChannels; i++)
01206         {
01207                 // Data channel header
01208                 std::string contentType = memread_string(bufferPos, 6);
01209                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Content type=" + contentType + ".", beVerboseHere);
01210                 float scaleFactor = memread_float(bufferPos);
01211                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scale factor=" + ::doubleToString(double(scaleFactor), 10) + ".", beVerboseHere);
01212                 float scaleOffset = memread_float(bufferPos);
01213                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scale offset=" + ::doubleToString(double(scaleOffset), 10) + ".", beVerboseHere);
01214                 INT32 startAngle = memread_INT32(bufferPos);                    // [1/10000 deg]
01215                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Start angle=" + ::doubleToString(double(startAngle)/10000.0, 2) + ".", beVerboseHere);
01216                 UINT32 angularResolution = memread_UINT16(bufferPos);   // [1/10000 deg]. Decoded value is UINT16, but needed as UINT32 later.
01217                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Angular resolution=" + ::doubleToString(double(angularResolution)/10000.0, 2) + ".", beVerboseHere);
01218 
01219                 // Decode the channel header
01220                 UINT16 layer = contentType.c_str()[4] - 'A';    // A, B, C or D --> 0..3
01221                 UINT16 echo = contentType.c_str()[5] - '1';             // 1, 2 or 3 --> 0..2
01222                 if (layer > 3)
01223                 {
01224                         printError("LdmrsSopasLayer::scanDataDecoder: We have decoded an invalid layer value of " + toString(layer) + ", allowed is 0..3.");
01225                         return;
01226                 }
01227                 if (echo > 2)
01228                 {
01229                         printError("LdmrsSopasLayer::scanDataDecoder: We have decoded an invalid echo value of " + toString(echo) + ", allowed is 0..2.");
01230                         return;
01231                 }
01232                 
01233                 // The data
01234                 UINT16 numOf16BitData = memread_UINT16(bufferPos);
01235                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of 16-bit data=" + ::toString(numOf16BitData) + ".", beVerboseHere);
01236                 for (UINT16 d = 0; d<numOf16BitData; d++)
01237                 {
01238                         //      Read the point
01239                         UINT16 data = memread_UINT16(bufferPos);
01240 //                              printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Data=" + ::toString(data) + ".", beVerboseHere);
01241                         
01242                         // Add the point only if it is valid.
01243                         if ((data > 0) && (data != 0xFFFF))
01244                         {
01245                                 // Create the new point structure in the scan, and fill it with the values
01246                                 ScanPoint& newPoint = scan->addNewPoint();
01247 
01248                                 // Horizontal angle
01249                                 double hAngle = ((startAngle + d * angularResolution) / 10000.0) * deg2rad;     // hAngle is in [rad]
01250 
01251                                 // Radial distance
01252                                 double dist = (double(data) * scaleFactor) / 1000.0;    // dist is in [m]
01253 
01254                                 // Set the dist and angle values
01255                                 newPoint.setPolar (dist, hAngle, vAngles[layer]);
01256 
01257                                 // Copy data to new scan point
01258                                 newPoint.setEchoWidth (0.0);
01259                                 newPoint.setFlags (0);
01260                                 newPoint.setSourceId (m_deviceId);
01261                                 newPoint.setLayer (layer);
01262                                 newPoint.setEchoNum (echo); // 0 or 1 or ...
01263                         }
01264                 }
01265         }
01266         
01267         // 8-bit data
01268         UINT16 numOf8BitDataChannels = memread_UINT16(bufferPos);
01269         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of 8-bit data channels=" + ::toString(numOf8BitDataChannels) + ".", beVerboseHere);
01270 
01271         // Position block (scanner coordinates)
01272         UINT16 numOfPositionBlocks = memread_UINT16(bufferPos);
01273         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of position blocks=" + ::toString(numOfPositionBlocks) + ".", beVerboseHere);
01274         if (numOfPositionBlocks == 1)
01275         {
01276                 float posX = memread_float(bufferPos);
01277                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Position X=" + ::doubleToString(double(posX), 2) + ".", beVerboseHere);
01278                 float posY = memread_float(bufferPos);
01279                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Position Y=" + ::doubleToString(double(posY), 2) + ".", beVerboseHere);
01280                 float posZ = memread_float(bufferPos);
01281                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Position Z=" + ::doubleToString(double(posZ), 2) + ".", beVerboseHere);
01282                 float rotX = memread_float(bufferPos);  // roll angle
01283                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Rot angle X=" + ::doubleToString(double(rotX), 2) + ".", beVerboseHere);
01284                 float rotY = memread_float(bufferPos);
01285                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Rot angle Y=" + ::doubleToString(double(rotY), 2) + ".", beVerboseHere);
01286                 float rotZ = memread_float(bufferPos);
01287                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Rot angle Z=" + ::doubleToString(double(rotZ), 2) + ".", beVerboseHere);
01288                 UINT8 rotMode = memread_UINT8(bufferPos);
01289                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Rot mode=" + ::toString(rotMode) + ".", beVerboseHere);
01290         }
01291         
01292         // Device name
01293         UINT16 numOfDeviceNames = memread_UINT16(bufferPos);
01294         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of device names=" + ::toString(numOfDeviceNames) + ".", beVerboseHere);
01295         if (numOfDeviceNames == 1)
01296         {
01297                 UINT16 deviceNameStringLen = memread_UINT16(bufferPos);
01298                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Device name string length=" + ::toString(deviceNameStringLen) + ".", beVerboseHere);
01299                 if (deviceNameStringLen > 16)
01300                 {
01301                         printError("LdmrsSopasLayer::scanDataDecoder: We have decoded an invalid device name string length of " +
01302                                                         toString(deviceNameStringLen) + ", allowed is 0..16.");
01303                         return;
01304                 }
01305                 std::string deviceName = memread_string(bufferPos, deviceNameStringLen);
01306         }
01307         
01308         // Comment block
01309         UINT16 numOfCommentBlocks = memread_UINT16(bufferPos);
01310         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Number of comment blocks=" + ::toString(numOfCommentBlocks) + ".", beVerboseHere);
01311         if (numOfCommentBlocks == 1)
01312         {
01313                 UINT16 commentStringLen = memread_UINT16(bufferPos);
01314                 printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Comment string length=" + ::toString(commentStringLen) + ".", beVerboseHere);
01315                 if (commentStringLen > 128)
01316                 {
01317                         printError("LdmrsSopasLayer::scanDataDecoder: We have decoded an invalid comment string length of " +
01318                                                         toString(commentStringLen) + ", allowed is 0..128.");
01319                         return;
01320                 }
01321                 std::string commentString = memread_string(bufferPos, commentStringLen);
01322         }
01323         
01324         //
01325         // Ignore the rest of the data as the MRS does not fill it with any information.
01326         //
01327 
01328         
01329         //
01330         // Set some information about the scanner. As we have very little information,
01331         // fill most values with "0.0".
01332         // For more information, use the LuxBase-Scandata instead.
01333         //
01334         // Create Scanner Info
01335         ScannerInfo si;
01336         si.setStartAngle(m_scanStartAngle);
01337         si.setEndAngle(m_scanEndAngle);
01338 //      printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scan start angle=" + ::doubleToString(scanStartAngle * rad2deg, 1) + ".", beVerboseHere);
01339 //      printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Scan end angle=" + ::doubleToString(scanEndAngle * rad2deg, 1) + ".", beVerboseHere);
01340 
01341         si.setScanFrequency(double(scanFrequency) / 256.0);
01342         si.setBeamTilt(0.0);                                    // For 8-layer devices, the beam tilt is different (either 1.2 or 1.6 deg)!
01343         si.setScanFlags(0);
01344         si.setScanNumber(scanCount);
01345         si.setDeviceID(m_deviceId);
01346         si.setScannerType(Sourcetype_LDMRS); // for compatibility, if no value is set in the scanner's config.
01347         Time start, end;
01348         start.set(double(systemTimeScan) / 1000000.0);                                                                                                  // No real information available
01349         end.set((double(systemTimeScan) / 1000000.0) + (1.0 / (double(scanFrequency) / 256.0)));                // No real information available
01350         si.setTimestamps(start, end);
01351 
01352         // Mounting position
01353         double yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ;
01354         yawAngle = 0.0; // No information available
01355         pitchAngle = 0.0;       // No information available
01356         rollAngle = 0.0;        // No information available
01357         offsetX = 0.0;  // No information available
01358         offsetY = 0.0;  // No information available
01359         offsetZ = 0.0;  // No information available
01360         Position3D mp(yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ);
01361         si.setMountingPosition(mp);
01362         scan->setScannerInfos(Scan::ScannerInfoVector(1, si));
01363 
01364         // Post this scan to the world above...
01365         scan->setSourceId(m_deviceId);
01366         m_manager->setDeviceData(scan);
01367 
01368         // All done
01369         printInfoMessage("LdmrsSopasLayer::scanDataDecoder: Done, leaving.", beVerboseHere);
01370 }
01371 
01372 
01373 //
01374 // Decodes incoming EvalCaseResult-Messages.
01375 //
01376 void LdmrsSopasLayer::evalCaseResultDecoder(SopasEventMessage& frame)
01377 {
01378         // cola-B by index
01379         printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: Called. Decoding SOPAS frame with length=" +
01380                                                 ::toString(frame.size()) + " bytes.", m_beVerbose);
01381 
01382         if (m_weWantFieldData == true)
01383         {
01384                 std::string receivedName;
01385                 UINT32 receiveIndex;
01386                 BYTE* bufferPos;
01387 
01388                 EvalCaseResults* evalCases;
01389                 evalCases = new EvalCaseResults();
01390                 evalCases->setSourceId(m_deviceId);
01391 
01392                 printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: Reading variable index.", m_beVerbose);
01393                 receiveIndex = frame.getVariableIndex(); // byte number 12 13
01394 
01395                 if (receiveIndex != index_event_aEvalCaseResult)
01396                 {
01397                         printError("LdmrsSopasLayer::evalCaseResultDecoder: This is not an eval case message, aborting!");
01398                         return;
01399                 }
01400                 else
01401                 {
01402                         printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: This is an eval case message.", m_beVerbose);
01403                 }
01404 
01405                 // Obviously field events are registered. This is just for global information.
01406                 m_fieldEventIsRegistered = true;
01407 
01408                 // Read the number of eval cases
01409                 bufferPos = &(frame.getPayLoad()[5]); // pay load without "sSI + index (2 byte)"
01410                 UINT16 cases = memread_UINT16 (bufferPos); // 14 15, array length in sopas is always decoded in uint16 !!!
01411                 printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: This frame contains " + ::toString(cases) + " cases to decode.", m_beVerbose);
01412 
01413                 UINT16 length;
01414                 for (UINT16 i = 0; i < cases; i++)
01415                 {
01416                         EvalCaseResult result;
01417 
01418                         result.uiVersionNo = memread_UINT16 (bufferPos); // 16 17
01419                         result.CaseHdr.usiNumber = memread_UINT8 (bufferPos); // 18
01420                         result.CaseHdr.udiSysCount = memread_UINT32 (bufferPos); // 19 20 21 22
01421                         result.CaseHdr.dDistScaleFactor = memread_float (bufferPos); // 23 24 25 26
01422                         result.CaseHdr.dDistScaleOffset = memread_float (bufferPos); // 27 28 29 30
01423                         result.CaseHdr.uiAngleScaleFactor = memread_UINT32 (bufferPos); // 31 32 33 34
01424                         result.CaseHdr.iAngleScaleOffset = memread_INT32 (bufferPos); // 35 36 37 38
01425                         UINT8 caseResultMRS = memread_UINT8 (bufferPos);
01426                         EvalCaseResult::CaseResult caseResult;
01427                         std::string resTxt;
01428                         switch (caseResultMRS)
01429                         {
01430                         case 1:
01431                                 caseResult = EvalCaseResult::ECR_DONT_CARE;
01432                                 resTxt = "dont_care";
01433                                 break;
01434                         case 2:
01435                                 // NOTE: LD-MRS signals always ECR_FALLING instead of LOW
01436                                 caseResult = EvalCaseResult::ECR_FALLING;
01437                                 caseResult = EvalCaseResult::ECR_LOW;
01438                                 resTxt = "low";
01439                                 break;
01440                         case 3:
01441                                 caseResult = EvalCaseResult::ECR_LOW;
01442                                 resTxt = "low";
01443                                 break;
01444                         case 4:
01445                                 caseResult = EvalCaseResult::ECR_DETECTING;
01446                                 resTxt = "detecting";
01447                                 break;
01448                         case 5:
01449                                 caseResult = EvalCaseResult::ECR_INVALID;
01450                                 resTxt = "invalid";
01451                                 break;
01452                         case 6:
01453                                 // NOTE: LD-MRS signals always ECR_RAISING instead of HIGH
01454                                 caseResult = EvalCaseResult::ECR_RAISING;
01455                                 caseResult = EvalCaseResult::ECR_HIGH;
01456                                 resTxt = "high";
01457                                 break;
01458                         case 7:
01459                                 caseResult = EvalCaseResult::ECR_HIGH;
01460                                 resTxt = "high";
01461                                 break;
01462                         default:
01463                                 caseResult = EvalCaseResult::ECR_INVALID;
01464                                 resTxt = "invalid";
01465                                 break;
01466                         }
01467                         result.m_eCaseResult = caseResult; // 39
01468                         printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: CaseResult is <" + resTxt + ">.", m_beVerbose);
01469 
01470                         // Dummy read for aFieldInfringement (always 0). Currently not in use, use the
01471                         // result from above instead.
01472                         length = memread_UINT16 (bufferPos); // 40 41
01473 //                      if (length == 0)
01474 //                      {
01475 //                              printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: FieldInfringement found.", m_beVerbose);
01476 //                              std::string fieldInfringement = colab::getStringFromBuffer(bufferPos, length);
01477 //                              printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: FieldInfringement: " + fieldInfringement + ".", m_beVerbose);
01478 //                      }
01479 
01480                         length = memread_UINT16 (bufferPos); // 42 43
01481                         if (length == 0)
01482                         {
01483                                 result.m_sCaseName = "";
01484                         }
01485                         else
01486                         {
01487                                 result.m_sCaseName = colab::getStringFromBuffer(bufferPos, length);
01488                                 printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: Case name: " + result.m_sCaseName + ".", m_beVerbose);
01489                         }
01490 
01491                         length = memread_UINT16 (bufferPos); // 44 45
01492                         if (length == 0)
01493                         {
01494                                 result.m_sComment = "";
01495                         }
01496                         else
01497                         {
01498                                 result.m_sComment = colab::getStringFromBuffer(bufferPos, length);
01499                                 printInfoMessage("LdmrsSopasLayer::evalCaseResultDecoder: Comment: "+ result.m_sComment + ".", m_beVerbose);
01500                         }
01501 
01502                         length = memread_UINT16 (bufferPos); // 46 47
01503                         if (length == 1)
01504                         {
01505                                 result.aTimeBlock.uiYear = memread_UINT16 (bufferPos);
01506                                 result.aTimeBlock.usiMonth = memread_UINT8 (bufferPos);
01507                                 result.aTimeBlock.usiDay = memread_UINT8 (bufferPos);
01508                                 result.aTimeBlock.usiHour = memread_UINT8 (bufferPos);
01509                                 result.aTimeBlock.usiMinute = memread_UINT8 (bufferPos);
01510                                 result.aTimeBlock.usiSec = memread_UINT8 (bufferPos);
01511                                 result.aTimeBlock.udiUSec = memread_UINT32 (bufferPos);
01512                         }
01513 
01514                         //traceDebug(LDMRS_VERSION) << result << std::endl;
01515                         evalCases->add(result);
01516                 }
01517 
01518                 // notify only in case of data change
01519                 if ((evalCases->size() > 0) &&
01520                         ((*evalCases) != m_lastEvalCaseResults))
01521                 {
01522                         m_lastEvalCaseResults = *evalCases;
01523                         m_manager->setDeviceData((BasicData*)evalCases);
01524                 }
01525         }
01526         else
01527         {
01528                 printWarning("LdmrsSopasLayer::evalCaseResultDecoder: Received eval case, but we do not want to listen to eval cases!");
01529 
01530                 unregisterEvent(index_event_aEvalCaseResult);
01531         }
01532 }
01533 
01534 //
01535 //
01536 //
01537 SensorStateInfo LdmrsSopasLayer::getSensorStateInfo()
01538 {
01539         SensorStateInfo sensorStateInfo;
01540         sensorStateInfo.setSourceId(m_deviceId);
01541         sensorStateInfo.setEvalCases(m_evalCases);
01542         sensorStateInfo.setFields(m_fields);
01543         
01544         MeasurementList measureList;
01545         measureList.setSourceId(m_deviceId);
01546         Measurement meas;
01547 //      measureList.setGroupName("SensorState");
01548 //      measureList.setListName("SensorStates");
01549         
01550         meas.m_measType = Meastype_DeviceName;
01551         meas.m_textValue = m_scannerName;
01552         measureList.m_list.push_back(meas);
01553         
01554         meas.m_measType = Meastype_DeviceVersion;
01555         meas.m_textValue = m_scannerVersion;
01556         measureList.m_list.push_back(meas);
01557         
01558         meas.m_measType = Meastype_ScanFreq;
01559         meas.m_doubleValue = m_scanFreq;
01560         measureList.m_list.push_back(meas);
01561         
01562         meas.m_measType = Meastype_ScanStartAngle;
01563         meas.m_doubleValue = m_scanStartAngle;
01564         measureList.m_list.push_back(meas);
01565 
01566         meas.m_measType = Meastype_ScanStopAngle;
01567         meas.m_doubleValue = m_scanEndAngle;
01568         measureList.m_list.push_back(meas);
01569 
01570         meas.m_measType = Meastype_ScanResolution;
01571         meas.m_doubleValue = m_angleResolution;
01572         measureList.m_list.push_back(meas);
01573         
01574 //      measureList.add(Measurement::Temperature, m_temperature);
01575 
01576         // no sensor state available
01577 //      std::bitset<16> stateBits(state);
01578 //      std::bitset<16> inputBits(inputs);
01579 //      std::bitset<16> outputBits(outputs);
01580 
01581         // build sensorStateInfo
01582         // traceDebug("") << "state: " << stateBits << " - inputs: " << inputBits << " - outputs: " << outputBits << std::endl;
01583 //      SensorStateInfo::StateMap stateMap;
01584 
01585 
01586 //      stateMap[STATENAME_DEVICE_ERROR] = stateBits[0];
01587 //      stateMap[STATENAME_CONTAMINATION_WARNING] = stateBits[1];
01588 //      stateMap[STATENAME_CONTAMINATION_ERROR] = stateBits[2];
01589 //      stateMap["N_TO_1_FILTER_ENABLED"] = m_bNto1FilterEnable;
01590 //      stateMap["PARTICLE_FILTER_ENABLED"] = m_particleFilter.bEnable;
01591 //      stateMap["MEAN_FILTER_ENABLED"] = m_meanFilter.bEnable;
01592 //      sensorStateInfo.setStateMap(stateMap);
01593 
01594 //      SensorStateInfo::StateVector inputStates(16, SensorStateInfo::OFF);
01595 //      SensorStateInfo::StateVector outputStates(16, SensorStateInfo::OFF);
01596 //      for (UINT32 j = 0; j < 16; ++j)
01597 //      {
01598 //              inputStates[j] = inputBits[j] ? SensorStateInfo::ON : SensorStateInfo::OFF;
01599 //              outputStates[j] = outputBits[j] ? SensorStateInfo::ON : SensorStateInfo::OFF;
01600 //      }
01601 //      sensorStateInfo.setInputStates(inputStates);
01602 //      sensorStateInfo.setOutputStates(outputStates);
01603 
01604 //      measureList.add(Measurement::SerialNumber, serialNo);
01605         sensorStateInfo.setMeasurementList(measureList);
01606         sensorStateInfo.setLastKnownEvalCaseResults(m_lastEvalCaseResults);
01607 
01608         return sensorStateInfo;
01609 }
01610 
01611 
01612 //
01613 // Encode a FieldParameter structure into CoLa-B-data. This structure can then be sent to the MRS to
01614 // parametrize one of the 16 possible fields.
01615 // Returns the number of bytes that have been written into the buffer.
01616 //
01617 UINT32 LdmrsSopasLayer::colaB_fieldEncoder(BYTE* buffer, const FieldParameter& field)
01618 {
01619         bool beVerboseHere = m_beVerbose;
01620 //      beVerboseHere = true;
01621         
01622         // Make a copy of the original buffer pos
01623         BYTE* bufferOrg = buffer;
01624         
01625         // Print some information about the field
01626         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: DistScaleFactor: " + toString(field.getDistScaleFactor(), 2) + ".", beVerboseHere);
01627         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: DistScaleOffset: " + toString(field.getDistScaleOffset(), 2) + ".", beVerboseHere);
01628         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: AngleScaleFactor: " + toString(field.getAngleScaleFactor()) + ".", beVerboseHere);
01629         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: AngleScaleOffset: " + toString(field.getAngleScaleOffset()) + ".", beVerboseHere);
01630         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: FieldTypeIntern: " + toString((INT32)(field.getFieldTypeIntern())) + ".", beVerboseHere);
01631         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: Field number: " + toString(field.getFieldNumber()) + ".", beVerboseHere);
01632 
01633         // Encode the first part
01634         memwrite_float(buffer, field.getDistScaleFactor());
01635         memwrite_float(buffer, field.getDistScaleOffset());
01636         memwrite_UINT32(buffer, field.getAngleScaleFactor());
01637         memwrite_INT32(buffer, field.getAngleScaleOffset());
01638         memwrite_UINT8(buffer, field.getFieldTypeIntern());
01639         memwrite_UINT8(buffer, field.getFieldNumber());
01640 
01641         // Segmented?
01642         if (field.getFieldType() == FieldDescription::Segmented)
01643         {
01644                 // Yes
01645                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: Writing a segmented field.", beVerboseHere);
01646                 // There is 1 segmented field structure
01647                 memwrite_UINT16(buffer, 1);
01648 
01649                 // Number of points
01650                 FieldSegmented* seg = (FieldSegmented*)field.getField();
01651                 memwrite_UINT16(buffer, seg->getNumberOfPoints());
01652 
01653                 // The points
01654                 for (UINT16 p=0; p<seg->getNumberOfPoints(); p++)
01655                 {
01656                         // Angle index (transform to 1/32° tics)
01657 //                      double angle = angleToRad(field.getAngleScaleOffset() + (double)angleIdx * field.getAngleScaleFactor());
01658                         double angle = seg->getPoints().at(p).getAngle() * rad2deg;
01659                         angle = -angle;
01660                         angle *= 32;
01661                         INT32 angleInt = (INT32)angle;
01662                         INT32 angleDiff = angleInt - field.getAngleScaleOffset();
01663                         angleDiff /= field.getAngleScaleFactor();
01664                         UINT16 angleIdx = (UINT16)angleDiff;
01665                         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: AngleIdx = " + toString(angleIdx) + ".", beVerboseHere);
01666 
01667                         memwrite_UINT16(buffer, angleIdx);
01668                         
01669                         // Start dist
01670                         double startDist = seg->getPoints().at(p).getStartDist();
01671                         if (startDist == NaN_double)
01672                         {
01673                                 // Invalid
01674                                 memwrite_UINT16(buffer, 0xFFFF);
01675                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: StartDist = invalid.", beVerboseHere);
01676                         }
01677                         else
01678                         {
01679                                 startDist *= 1000;
01680                                 startDist /= field.getDistScaleFactor();
01681                                 startDist -= field.getDistScaleOffset();
01682                                 UINT16 startDistInt = (UINT16)startDist;
01683                                 memwrite_UINT16(buffer, startDistInt);
01684                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: StartDist = " + toString(startDistInt) + ".", beVerboseHere);
01685                         }
01686                         
01687                         // End dist
01688                         double endDist = seg->getPoints().at(p).getEndDist();
01689                         if (endDist == NaN_double)
01690                         {
01691                                 // Invalid
01692                                 memwrite_UINT16(buffer, 0xFFFF);
01693                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: EndDist = invalid.", beVerboseHere);
01694                         }
01695                         else
01696                         {
01697                                 endDist *= 1000;
01698                                 endDist /= field.getDistScaleFactor();
01699                                 endDist -= field.getDistScaleOffset();
01700                                 UINT16 endDistInt = (UINT16)endDist;
01701                                 memwrite_UINT16(buffer, endDistInt);
01702                                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: EndDist = " + toString(endDistInt) + ".", beVerboseHere);
01703                         }
01704                 }
01705         }
01706         else
01707         {
01708                 // It is not a segmented field
01709                 memwrite_UINT16(buffer, 0);
01710         }
01711         
01712         // Rectangular?
01713         if (field.getFieldType() == FieldDescription::Rectangle)
01714         {
01715                 printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: Writing rectangular field.", beVerboseHere);
01716                 memwrite_UINT16(buffer, 1);
01717 
01718                 FieldRectangle* rect = (FieldRectangle*)field.getField();
01719                 
01720                 // RefPointAngle
01721                 INT32 refPointAngle = rect->getRefPointAngle() * rad2deg * 32.0;        // ) .getField(). = memread_INT32(bufferPos);
01722                 refPointAngle = -refPointAngle;
01723                 memwrite_INT32(buffer, refPointAngle);
01724 
01725                 // RefPointDist
01726                 UINT16 refPointDist = (UINT16)(((rect->getRefPointDist() * 1000.0) / field.getDistScaleFactor()) + field.getDistScaleOffset());
01727                 memwrite_UINT16(buffer, refPointDist);
01728 
01729                 // RotAngle
01730                 INT32 rotAngle = rect->getRotAngle() * rad2deg * 32.0;
01731                 rotAngle = -rotAngle;
01732                 memwrite_INT32(buffer, rotAngle);
01733 
01734                 // Length
01735                 UINT32 length = rect->getLength() * 1000;
01736                 memwrite_UINT32(buffer, length);
01737                 
01738                 // Width
01739                 UINT32 width = rect->getWidth() * 1000;
01740                 memwrite_UINT32(buffer, width);
01741         }
01742         else
01743         {
01744                 // It is not a rectangular field
01745                 memwrite_UINT16(buffer, 0);
01746         }
01747 
01748         // It is not a radial field - the MRS does not support radial fields
01749         memwrite_UINT16(buffer, 0);
01750         
01751         // It is not a dynamic field - the MRS does not support dynamic fields.
01752         memwrite_UINT16(buffer, 0);
01753 
01754         // Version number
01755         memwrite_UINT16(buffer, (UINT16)field.getVersionNumber());
01756         
01757         // Field name length
01758         memwrite_UINT16(buffer, (UINT16)field.getFieldName().length());
01759 
01760         // Field name
01761         if (field.getFieldName().length() > 0)
01762         {
01763                 memwrite_string(buffer, field.getFieldName());
01764         }
01765 
01766         // Comment length
01767         memwrite_UINT16(buffer, (UINT16)field.getComment().length());
01768 
01769         // Comment
01770         if (field.getComment().length() > 0)
01771         {
01772                 memwrite_string(buffer, field.getComment());
01773         }
01774         
01775         // enable layer filter
01776         memwrite_UINT8(buffer, 0x00);   // 0 = disabled, 1 = enabled
01777         
01778         // layer bit field
01779         memwrite_UINT16(buffer, 0x000F);        // 0x000F = all layers enabled
01780 
01781         // How many bytes have been used?
01782         UINT32 len = (UINT32)((UINT64)buffer - (UINT64)bufferOrg);
01783         
01784         printInfoMessage("LdmrsSopasLayer::colaB_fieldEncoder: Encoded " + toString(len) + " bytes. All done, leaving.", beVerboseHere);
01785         
01786         //
01787         // Debug
01788         //
01789 //      traceBuffer("Field buffer contents:", bufferOrg, len);
01790         
01791         // Debug: Decode this set of data
01792 //      SopasAnswer a(bufferOrg, len);
01793 //      colaB_fieldDecoder(&a);
01794         
01795         return len;
01796 }
01797 
01798 
01799 }       // namespace devices


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Thu Jun 6 2019 21:02:36