public.cc
Go to the documentation of this file.
00001 
00037 #include <stdlib.h>
00038 #include <algorithm>
00039 
00040 #include "details/utility/Functional.hh"
00041 
00042 #include "details/channel.hh"
00043 #include "details/query.hh"
00044 
00045 #include "details/wire/VersionRequestMessage.h"
00046 #include "details/wire/VersionResponseMessage.h"
00047 #include "details/wire/StatusRequestMessage.h"
00048 #include "details/wire/StatusResponseMessage.h"
00049 
00050 #include "details/wire/StreamControlMessage.h"
00051 #include "details/wire/SysGetDirectedStreamsMessage.h"
00052 #include "details/wire/SysDirectedStreamsMessage.h"
00053 
00054 #include "details/wire/CamControlMessage.h"
00055 #include "details/wire/CamSetResolutionMessage.h"
00056 #include "details/wire/CamGetConfigMessage.h"
00057 #include "details/wire/CamConfigMessage.h"
00058 #include "details/wire/CamSetTriggerSourceMessage.h"
00059 
00060 #include "details/wire/LidarSetMotorMessage.h"
00061 
00062 #include "details/wire/LedGetStatusMessage.h"
00063 #include "details/wire/LedSetMessage.h"
00064 #include "details/wire/LedStatusMessage.h"
00065 
00066 #include "details/wire/LedGetSensorStatusMessage.h"
00067 #include "details/wire/LedSensorStatusMessage.h"
00068 
00069 #include "details/wire/SysMtuMessage.h"
00070 #include "details/wire/SysGetMtuMessage.h"
00071 #include "details/wire/SysFlashOpMessage.h"
00072 #include "details/wire/SysGetNetworkMessage.h"
00073 #include "details/wire/SysNetworkMessage.h"
00074 #include "details/wire/SysGetDeviceInfoMessage.h"
00075 #include "details/wire/SysDeviceInfoMessage.h"
00076 #include "details/wire/SysGetCameraCalibrationMessage.h"
00077 #include "details/wire/SysCameraCalibrationMessage.h"
00078 #include "details/wire/SysGetSensorCalibrationMessage.h"
00079 #include "details/wire/SysGetTransmitDelayMessage.h"
00080 #include "details/wire/SysSensorCalibrationMessage.h"
00081 #include "details/wire/SysTransmitDelayMessage.h"
00082 #include "details/wire/SysGetLidarCalibrationMessage.h"
00083 #include "details/wire/SysLidarCalibrationMessage.h"
00084 #include "details/wire/SysGetDeviceModesMessage.h"
00085 #include "details/wire/SysDeviceModesMessage.h"
00086 #include "details/wire/SysGetExternalCalibrationMessage.h"
00087 #include "details/wire/SysExternalCalibrationMessage.h"
00088 
00089 #include "details/wire/ImuGetInfoMessage.h"
00090 #include "details/wire/ImuGetConfigMessage.h"
00091 #include "details/wire/ImuInfoMessage.h"
00092 #include "details/wire/ImuConfigMessage.h"
00093 
00094 #include "details/wire/SysTestMtuMessage.h"
00095 #include "details/wire/SysTestMtuResponseMessage.h"
00096 
00097 namespace crl {
00098 namespace multisense {
00099 namespace details {
00100 
00101 //
00102 // The user may "hold on" to the buffer back-end
00103 // of a datum within a callback thread.
00104 
00105 CRL_THREAD_LOCAL utility::BufferStream *dispatchBufferReferenceTP = NULL;
00106 
00107 //
00108 //
00109 // Public API follows
00110 //
00111 //
00112 
00113 //
00114 // Adds a new image listener
00115 
00116 Status impl::addIsolatedCallback(image::Callback callback, 
00117                                  DataSource     imageSourceMask,
00118                                  void           *userDataP)
00119 {
00120     try {
00121 
00122         utility::ScopedLock lock(m_dispatchLock);
00123         m_imageListeners.push_back(new ImageListener(callback, 
00124                                                      imageSourceMask, 
00125                                                      userDataP,
00126                                                      MAX_USER_IMAGE_QUEUE_SIZE));
00127 
00128     } catch (const std::exception& e) {
00129         CRL_DEBUG("exception: %s\n", e.what());
00130         return Status_Exception;
00131     }
00132     return Status_Ok;
00133 }
00134 
00135 //
00136 // Adds a new laser listener
00137 
00138 Status impl::addIsolatedCallback(lidar::Callback callback, 
00139                                  void           *userDataP)
00140 {
00141     try {
00142 
00143         utility::ScopedLock lock(m_dispatchLock);
00144         m_lidarListeners.push_back(new LidarListener(callback, 
00145                                                      0,
00146                                                      userDataP,
00147                                                      MAX_USER_LASER_QUEUE_SIZE));
00148 
00149     } catch (const std::exception& e) {
00150         CRL_DEBUG("exception: %s\n", e.what());
00151         return Status_Exception;
00152     }
00153     return Status_Ok;
00154 }
00155 
00156 //
00157 // Adds a new PPS listener
00158 
00159 Status impl::addIsolatedCallback(pps::Callback callback, 
00160                                  void         *userDataP)
00161 {
00162     try {
00163 
00164         utility::ScopedLock lock(m_dispatchLock);
00165         m_ppsListeners.push_back(new PpsListener(callback, 
00166                                                  0,
00167                                                  userDataP,
00168                                                  MAX_USER_PPS_QUEUE_SIZE));
00169 
00170     } catch (const std::exception& e) {
00171         CRL_DEBUG("exception: %s\n", e.what());
00172         return Status_Exception;
00173     }
00174     return Status_Ok;
00175 }
00176 
00177 //
00178 // Adds a new IMU listener
00179 
00180 Status impl::addIsolatedCallback(imu::Callback callback, 
00181                                  void         *userDataP)
00182 {
00183     try {
00184 
00185         utility::ScopedLock lock(m_dispatchLock);
00186         m_imuListeners.push_back(new ImuListener(callback, 
00187                                                  0,
00188                                                  userDataP,
00189                                                  MAX_USER_IMU_QUEUE_SIZE));
00190 
00191     } catch (const std::exception& e) {
00192         CRL_DEBUG("exception: %s\n", e.what());
00193         return Status_Exception;
00194     }
00195     return Status_Ok;
00196 }
00197 
00198 //
00199 // Removes an image listener
00200 
00201 Status impl::removeIsolatedCallback(image::Callback callback)
00202 {
00203     try {
00204         utility::ScopedLock lock(m_dispatchLock);
00205 
00206         std::list<ImageListener*>::iterator it;
00207         for(it  = m_imageListeners.begin();
00208             it != m_imageListeners.end();
00209             it ++) {
00210         
00211             if ((*it)->callback() == callback) {
00212                 delete *it;
00213                 m_imageListeners.erase(it);
00214                 return Status_Ok;
00215             }
00216         }
00217 
00218     } catch (const std::exception& e) {
00219         CRL_DEBUG("exception: %s\n", e.what());
00220         return Status_Exception;
00221     }
00222 
00223     return Status_Error;
00224 }
00225 
00226 //
00227 // Removes a lidar listener
00228 
00229 Status impl::removeIsolatedCallback(lidar::Callback callback)
00230 {
00231     try {
00232         utility::ScopedLock lock(m_dispatchLock);
00233 
00234         std::list<LidarListener*>::iterator it;
00235         for(it  = m_lidarListeners.begin();
00236             it != m_lidarListeners.end();
00237             it ++) {
00238         
00239             if ((*it)->callback() == callback) {
00240                 delete *it;
00241                 m_lidarListeners.erase(it);
00242                 return Status_Ok;
00243             }
00244         }
00245 
00246     } catch (const std::exception& e) {
00247         CRL_DEBUG("exception: %s\n", e.what());
00248         return Status_Exception;
00249     }
00250 
00251     return Status_Error;
00252 }
00253 
00254 //
00255 // Removes a PPS listener
00256 
00257 Status impl::removeIsolatedCallback(pps::Callback callback)
00258 {
00259     try {
00260         utility::ScopedLock lock(m_dispatchLock);
00261 
00262         std::list<PpsListener*>::iterator it;
00263         for(it  = m_ppsListeners.begin();
00264             it != m_ppsListeners.end();
00265             it ++) {
00266         
00267             if ((*it)->callback() == callback) {
00268                 delete *it;
00269                 m_ppsListeners.erase(it);
00270                 return Status_Ok;
00271             }
00272         }
00273 
00274     } catch (const std::exception& e) {
00275         CRL_DEBUG("exception: %s\n", e.what());
00276         return Status_Exception;
00277     }
00278 
00279     return Status_Error;
00280 }
00281 
00282 //
00283 // Removes an IMU listener
00284 
00285 Status impl::removeIsolatedCallback(imu::Callback callback)
00286 {
00287     try {
00288         utility::ScopedLock lock(m_dispatchLock);
00289 
00290         std::list<ImuListener*>::iterator it;
00291         for(it  = m_imuListeners.begin();
00292             it != m_imuListeners.end();
00293             it ++) {
00294         
00295             if ((*it)->callback() == callback) {
00296                 delete *it;
00297                 m_imuListeners.erase(it);
00298                 return Status_Ok;
00299             }
00300         }
00301 
00302     } catch (const std::exception& e) {
00303         CRL_DEBUG("exception: %s\n", e.what());
00304         return Status_Exception;
00305     }
00306 
00307     return Status_Error;
00308 }
00309 
00310 //
00311 // Reserve the current callback buffer being used in a dispatch thread
00312 
00313 void *impl::reserveCallbackBuffer()
00314 {
00315     if (dispatchBufferReferenceTP)  {
00316 
00317         try {
00318 
00319             return reinterpret_cast<void*>(new utility::BufferStream(*dispatchBufferReferenceTP));
00320             
00321         } catch (const std::exception& e) {
00322             
00323             CRL_DEBUG("exception: %s\n", e.what());
00324             
00325         } catch (...) {
00326             
00327             CRL_DEBUG("unknown exception\n");
00328         }
00329     }
00330 
00331     return NULL;
00332 }
00333 
00334 //
00335 // Release a user reserved buffer back to us
00336 
00337 Status impl::releaseCallbackBuffer(void *referenceP)
00338 {
00339     if (referenceP) {
00340         try {
00341 
00342             delete reinterpret_cast<utility::BufferStream*>(referenceP);
00343             return Status_Ok;
00344 
00345         } catch (const std::exception& e) {
00346             
00347             CRL_DEBUG("exception: %s\n", e.what());
00348             return Status_Exception;
00349             
00350         } catch (...) {
00351             
00352             CRL_DEBUG("unknown exception\n");
00353             return Status_Exception;
00354         }
00355     }
00356 
00357     return Status_Error;
00358 }
00359 
00360 //
00361 // Get a copy of the histogram for a particular frame ID
00362 
00363 Status impl::getImageHistogram(int64_t           frameId,
00364                                image::Histogram& histogram)
00365 {
00366     try {
00367         
00368         utility::ScopedLock lock(m_imageMetaCache.mutex());
00369 
00370         const wire::ImageMeta *metaP = m_imageMetaCache.find_nolock(frameId);
00371         if (NULL == metaP) {
00372             CRL_DEBUG("no meta cached for frameId %ld", 
00373                       static_cast<long int>(frameId));
00374             return Status_Failed;
00375         }
00376 
00377         histogram.channels = wire::ImageMeta::HISTOGRAM_CHANNELS;
00378         histogram.bins     = wire::ImageMeta::HISTOGRAM_BINS;
00379 
00380         const int entries   = histogram.channels * histogram.bins;
00381         const int sizeBytes = entries * sizeof(uint32_t);
00382 
00383         histogram.data.resize(entries);
00384         memcpy(&(histogram.data[0]), metaP->histogramP, sizeBytes);
00385 
00386         return Status_Ok;
00387 
00388     } catch (const std::exception& e) {
00389         CRL_DEBUG("exception: %s\n", e.what());
00390         return Status_Exception;
00391     }
00392 
00393     return Status_Error;
00394 }
00395 
00396 //
00397 // Enable/disable network-based time synchronization
00398 
00399 Status impl::networkTimeSynchronization(bool enabled)
00400 {
00401     m_networkTimeSyncEnabled = enabled;
00402     return Status_Ok;
00403 }
00404 
00405 //
00406 // Primary stream control
00407 
00408 Status impl::startStreams(DataSource mask)
00409 {
00410     utility::ScopedLock lock(m_streamLock);
00411 
00412     wire::StreamControl cmd;
00413 
00414     cmd.enable(sourceApiToWire(mask));
00415 
00416     Status status = waitAck(cmd);
00417     if (Status_Ok == status)
00418         m_streamsEnabled |= mask;
00419 
00420     return status;
00421 }
00422 Status impl::stopStreams(DataSource mask)
00423 {
00424     utility::ScopedLock lock(m_streamLock);
00425 
00426     wire::StreamControl cmd;
00427 
00428     cmd.disable(sourceApiToWire(mask));
00429 
00430     Status status = waitAck(cmd);
00431     if (Status_Ok == status)
00432         m_streamsEnabled &= ~mask;
00433 
00434     return status;
00435 }
00436 Status impl::getEnabledStreams(DataSource& mask)
00437 {
00438     utility::ScopedLock lock(m_streamLock);
00439 
00440     mask = m_streamsEnabled;
00441 
00442     return Status_Ok;
00443 }
00444 
00445 //
00446 // Secondary stream control
00447 
00448 Status impl::startDirectedStream(const DirectedStream& stream)
00449 {
00450     wire::SysDirectedStreams cmd(wire::SysDirectedStreams::CMD_START);
00451 
00452     cmd.streams.push_back(wire::DirectedStream(stream.mask,
00453                                                stream.address,
00454                                                stream.udpPort,
00455                                                stream.fpsDecimation));
00456     return waitAck(cmd);
00457 }
00458 
00459 Status impl::startDirectedStreams(const std::vector<DirectedStream>& streams)
00460 {
00461     wire::SysDirectedStreams cmd(wire::SysDirectedStreams::CMD_START);
00462 
00463     for (uint32_t index = 0 ; index < streams.size() ; ++index) {
00464         DirectedStream stream = streams[index];
00465 
00466         cmd.streams.push_back(wire::DirectedStream(stream.mask,
00467                                                    stream.address,
00468                                                    stream.udpPort,
00469                                                    stream.fpsDecimation));
00470     }
00471     return waitAck(cmd);
00472 }
00473 
00474 Status impl::stopDirectedStream(const DirectedStream& stream)
00475 {
00476     wire::SysDirectedStreams cmd(wire::SysDirectedStreams::CMD_STOP);
00477 
00478     cmd.streams.push_back(wire::DirectedStream(stream.mask,
00479                                                stream.address,
00480                                                stream.udpPort,
00481                                                stream.fpsDecimation));
00482     return waitAck(cmd);
00483 }
00484 
00485 Status impl::getDirectedStreams(std::vector<DirectedStream>& streams)
00486 {
00487     Status                   status;
00488     wire::SysDirectedStreams rsp;
00489 
00490     streams.clear();
00491 
00492     status = waitData(wire::SysGetDirectedStreams(), rsp);
00493     if (Status_Ok != status)
00494         return status;
00495     
00496     std::vector<wire::DirectedStream>::const_iterator it = rsp.streams.begin();
00497     for(; it != rsp.streams.end(); ++it)
00498         streams.push_back(DirectedStream((*it).mask,
00499                                          (*it).address,
00500                                          (*it).udpPort,
00501                                          (*it).fpsDecimation));
00502     return Status_Ok;
00503 }
00504 
00505 Status impl::maxDirectedStreams(uint32_t& maximum)
00506 {
00507     maximum = MAX_DIRECTED_STREAMS;
00508     return Status_Ok;
00509 }
00510 
00511 //
00512 // Set the trigger source
00513 
00514 Status impl::setTriggerSource(TriggerSource s)
00515 {
00516     uint32_t wireSource;
00517 
00518     switch(s) {
00519     case Trigger_Internal: 
00520 
00521         wireSource = wire::CamSetTriggerSource::SOURCE_INTERNAL;
00522         break;
00523 
00524     case Trigger_External:
00525 
00526         wireSource = wire::CamSetTriggerSource::SOURCE_EXTERNAL;
00527         break;
00528 
00529     default:
00530 
00531         return Status_Error;
00532     }
00533 
00534     return waitAck(wire::CamSetTriggerSource(wireSource));
00535 }
00536 
00537 //
00538 // Set the motor speed
00539 
00540 Status impl::setMotorSpeed(float rpm)
00541 {
00542     return waitAck(wire::LidarSetMotor(rpm));
00543 }
00544 
00545 //
00546 // Get/set the lighting configuration
00547 
00548 Status impl::getLightingConfig(lighting::Config& c)
00549 {
00550     Status          status;
00551     wire::LedStatus data;
00552 
00553     status = waitData(wire::LedGetStatus(), data);
00554     if (Status_Ok != status)
00555         return status;
00556         
00557     for(uint32_t i=0; i<lighting::MAX_LIGHTS; i++) {
00558         float duty=0.0f;
00559         if ((1<<i) & data.available)
00560             duty = (data.intensity[i] * 100.0f) / 255;
00561         c.setDutyCycle(i, duty);
00562     }
00563 
00564     c.setFlash(data.flash != 0);
00565 
00566     return Status_Ok;
00567 }
00568 
00569 Status impl::setLightingConfig(const lighting::Config& c)
00570 {
00571     wire::LedSet msg;
00572 
00573     msg.flash = c.getFlash() ? 1 : 0;
00574     for(uint32_t i=0; i<lighting::MAX_LIGHTS; i++) {
00575       
00576         float duty = c.getDutyCycle(i);
00577         if (duty >= 0.0f) {  // less than zero == not set
00578             msg.mask |= (1<<i);
00579             msg.intensity[i] = static_cast<uint8_t> (255.0f * (utility::boundValue(duty, 0.0f, 100.0f) / 100.0f));
00580         }
00581     }
00582 
00583     return waitAck(msg);
00584 }
00585 
00586 Status impl::getLightingSensorStatus(lighting::SensorStatus& status)
00587 {
00588     Status          requestStatus;
00589     wire::LedSensorStatus data;
00590 
00591     requestStatus = waitData(wire::LedGetSensorStatus(), data);
00592     if (Status_Ok != requestStatus)
00593         return requestStatus;
00594 
00595     status.ambientLightPercentage = data.ambientLightPercentage;
00596 
00597     return Status_Ok;
00598 }
00599 
00600 //
00601 // Requests version from sensor
00602 
00603 Status impl::getSensorVersion(VersionType& version)
00604 {
00605     version = m_sensorVersion.firmwareVersion;
00606     return Status_Ok;
00607 }
00608 
00609 //
00610 // Version of this API
00611 
00612 Status impl::getApiVersion(VersionType& version)
00613 {
00614     version = API_VERSION;
00615     return Status_Ok;
00616 }
00617 
00618 //
00619 // Requests all versioning information
00620 
00621 Status impl::getVersionInfo(system::VersionInfo& v)
00622 {
00623     v.apiBuildDate            = std::string(__DATE__ ", " __TIME__);
00624     v.apiVersion              = API_VERSION;
00625     v.sensorFirmwareBuildDate = m_sensorVersion.firmwareBuildDate;
00626     v.sensorFirmwareVersion   = m_sensorVersion.firmwareVersion;
00627     v.sensorHardwareVersion   = m_sensorVersion.hardwareVersion;
00628     v.sensorHardwareMagic     = m_sensorVersion.hardwareMagic;
00629     v.sensorFpgaDna           = m_sensorVersion.fpgaDna;
00630 
00631     return Status_Ok;
00632 }
00633 
00634 //
00635 // Query camera configuration
00636 
00637 Status impl::getImageConfig(image::Config& config)
00638 {
00639     Status          status;
00640     wire::CamConfig d;
00641 
00642     status = waitData(wire::CamGetConfig(), d);
00643     if (Status_Ok != status)
00644         return status;
00645 
00646     // for access to protected calibration members
00647     class ConfigAccess : public image::Config {
00648     public:
00649         void setCal(float fx, float fy, float cx, float cy,
00650                     float tx, float ty, float tz,
00651                     float r,  float p,  float w) {
00652             m_fx = fx; m_fy = fy; m_cx = cx; m_cy = cy;
00653             m_tx = tx; m_ty = ty; m_tz = tz; 
00654             m_roll = r; m_pitch = p; m_yaw = w;
00655         };
00656     };
00657       
00658     // what is the proper c++ cast for this?
00659     ConfigAccess& a = *((ConfigAccess *) &config);
00660     
00661     a.setResolution(d.width, d.height);
00662     if (-1 == d.disparities) { // pre v2.3 firmware
00663         if (1024 == d.width)   // TODO: check for monocular
00664             d.disparities = 128;
00665         else
00666             d.disparities = 0;
00667     }
00668     a.setDisparities(d.disparities);
00669     a.setFps(d.framesPerSecond);
00670     a.setGain(d.gain);
00671     
00672     a.setExposure(d.exposure);
00673     a.setAutoExposure(d.autoExposure != 0);
00674     a.setAutoExposureMax(d.autoExposureMax);
00675     a.setAutoExposureDecay(d.autoExposureDecay);
00676     a.setAutoExposureThresh(d.autoExposureThresh);
00677     
00678     a.setWhiteBalance(d.whiteBalanceRed, d.whiteBalanceBlue);
00679     a.setAutoWhiteBalance(d.autoWhiteBalance != 0);
00680     a.setAutoWhiteBalanceDecay(d.autoWhiteBalanceDecay);
00681     a.setAutoWhiteBalanceThresh(d.autoWhiteBalanceThresh);
00682     a.setStereoPostFilterStrength(d.stereoPostFilterStrength);
00683     a.setHdr(d.hdrEnabled);
00684 
00685     a.setCal(d.fx, d.fy, d.cx, d.cy, 
00686              d.tx, d.ty, d.tz,
00687              d.roll, d.pitch, d.yaw);
00688     
00689     return Status_Ok;
00690 }
00691 
00692 //
00693 // Set camera configuration
00694 //
00695 // Currently several sensor messages are combined and presented
00696 // to the user as one.
00697 
00698 Status impl::setImageConfig(const image::Config& c)
00699 {
00700     Status status;
00701 
00702     status = waitAck(wire::CamSetResolution(c.width(), 
00703                                             c.height(),
00704                                             c.disparities(),
00705                                             c.camMode(),
00706                                             c.offset()));
00707     if (Status_Ok != status)
00708         return status;
00709 
00710     wire::CamControl cmd;
00711 
00712     cmd.framesPerSecond = c.fps();
00713     cmd.gain            = c.gain();
00714     
00715     cmd.exposure           = c.exposure();
00716     cmd.autoExposure       = c.autoExposure() ? 1 : 0;
00717     cmd.autoExposureMax    = c.autoExposureMax();
00718     cmd.autoExposureDecay  = c.autoExposureDecay();
00719     cmd.autoExposureThresh = c.autoExposureThresh();
00720 
00721     cmd.whiteBalanceRed          = c.whiteBalanceRed();
00722     cmd.whiteBalanceBlue         = c.whiteBalanceBlue();
00723     cmd.autoWhiteBalance         = c.autoWhiteBalance() ? 1 : 0;
00724     cmd.autoWhiteBalanceDecay    = c.autoWhiteBalanceDecay();
00725     cmd.autoWhiteBalanceThresh   = c.autoWhiteBalanceThresh();
00726     cmd.stereoPostFilterStrength = c.stereoPostFilterStrength();
00727     cmd.hdrEnabled               = c.hdrEnabled();
00728 
00729     return waitAck(cmd);
00730 }
00731 
00732 //
00733 // Get camera calibration
00734 
00735 Status impl::getImageCalibration(image::Calibration& c)
00736 {
00737     wire::SysCameraCalibration d;
00738 
00739     Status status = waitData(wire::SysGetCameraCalibration(), d);
00740     if (Status_Ok != status)
00741         return status;
00742 
00743     CPY_ARRAY_2(c.left.M, d.left.M, 3, 3);
00744     CPY_ARRAY_1(c.left.D, d.left.D, 8);
00745     CPY_ARRAY_2(c.left.R, d.left.R, 3, 3);
00746     CPY_ARRAY_2(c.left.P, d.left.P, 3, 4);
00747 
00748     CPY_ARRAY_2(c.right.M, d.right.M, 3, 3);
00749     CPY_ARRAY_1(c.right.D, d.right.D, 8);
00750     CPY_ARRAY_2(c.right.R, d.right.R, 3, 3);
00751     CPY_ARRAY_2(c.right.P, d.right.P, 3, 4);
00752 
00753     return Status_Ok;
00754 }
00755 
00756 //
00757 // Set camera calibration
00758 
00759 Status impl::setImageCalibration(const image::Calibration& c)
00760 {
00761     wire::SysCameraCalibration d;
00762 
00763     CPY_ARRAY_2(d.left.M, c.left.M, 3, 3);
00764     CPY_ARRAY_1(d.left.D, c.left.D, 8);
00765     CPY_ARRAY_2(d.left.R, c.left.R, 3, 3);
00766     CPY_ARRAY_2(d.left.P, c.left.P, 3, 4);
00767 
00768     CPY_ARRAY_2(d.right.M, c.right.M, 3, 3);
00769     CPY_ARRAY_1(d.right.D, c.right.D, 8);
00770     CPY_ARRAY_2(d.right.R, c.right.R, 3, 3);
00771     CPY_ARRAY_2(d.right.P, c.right.P, 3, 4);
00772 
00773     return waitAck(d);
00774 }
00775 
00776 //
00777 // Get sensor calibration
00778 
00779 Status impl::getSensorCalibration(image::SensorCalibration& c)
00780 {
00781     wire::SysSensorCalibration d;
00782 
00783     Status status = waitData(wire::SysGetSensorCalibration(), d);
00784     if (Status_Ok != status)
00785         return status;
00786     CPY_ARRAY_1(c.adc_gain, d.adc_gain, 2);
00787     CPY_ARRAY_1(c.bl_offset, d.bl_offset, 2);
00788 
00789     return Status_Ok;
00790 }
00791 
00792 //
00793 // Set sensor calibration
00794 
00795 Status impl::setSensorCalibration(const image::SensorCalibration& c)
00796 {
00797     wire::SysSensorCalibration d;
00798 
00799     CPY_ARRAY_1(d.adc_gain, c.adc_gain, 2);
00800     CPY_ARRAY_1(d.bl_offset, c.bl_offset, 2);
00801 
00802     return waitAck(d);
00803 }
00804 
00805 //
00806 // Get sensor calibration
00807 
00808 Status impl::getTransmitDelay(image::TransmitDelay& c)
00809 {
00810     wire::SysTransmitDelay d;
00811 
00812     Status status = waitData(wire::SysGetTransmitDelay(), d);
00813     if (Status_Ok != status)
00814         return status;
00815     c.delay = d.delay;
00816 
00817     return Status_Ok;
00818 }
00819 
00820 //
00821 // Set sensor calibration
00822 
00823 Status impl::setTransmitDelay(const image::TransmitDelay& c)
00824 {
00825     wire::SysTransmitDelay d;
00826 
00827     d.delay = c.delay;;
00828 
00829     return waitAck(d);
00830 }
00831 
00832 //
00833 // Get lidar calibration
00834 
00835 Status impl::getLidarCalibration(lidar::Calibration& c)
00836 {
00837     wire::SysLidarCalibration d;
00838 
00839     Status status = waitData(wire::SysGetLidarCalibration(), d);
00840     if (Status_Ok != status)
00841         return status;
00842 
00843     CPY_ARRAY_2(c.laserToSpindle, d.laserToSpindle, 4, 4);
00844     CPY_ARRAY_2(c.cameraToSpindleFixed, d.cameraToSpindleFixed, 4, 4);
00845 
00846     return Status_Ok;
00847 }
00848 
00849 //
00850 // Set lidar calibration
00851 
00852 Status impl::setLidarCalibration(const lidar::Calibration& c)
00853 {
00854     wire::SysLidarCalibration d;
00855 
00856     CPY_ARRAY_2(d.laserToSpindle, c.laserToSpindle, 4, 4);
00857     CPY_ARRAY_2(d.cameraToSpindleFixed, c.cameraToSpindleFixed, 4, 4);
00858 
00859     return waitAck(d);
00860 }
00861 
00862 //
00863 // Get a list of supported image formats / data sources
00864 
00865 Status impl::getDeviceModes(std::vector<system::DeviceMode>& modes)
00866 {
00867     wire::SysDeviceModes d;
00868 
00869     Status status = waitData(wire::SysGetDeviceModes(), d);
00870     if (Status_Ok != status)
00871         return Status_Error;
00872 
00873     modes.resize(d.modes.size());
00874     for(uint32_t i=0; i<d.modes.size(); i++) {
00875 
00876         system::DeviceMode&     a = modes[i]; 
00877         const wire::DeviceMode& w = d.modes[i];
00878 
00879         a.width                = w.width;
00880         a.height               = w.height;
00881         a.supportedDataSources = sourceWireToApi(w.supportedDataSources);
00882         if (m_sensorVersion.firmwareVersion >= 0x0203)
00883             a.disparities = w.disparities;
00884         else 
00885             a.disparities = (a.width == 1024) ? 128 : 0;
00886     }
00887                         
00888     return Status_Ok;
00889 }
00890 
00891 //
00892 // Set/get the mtu
00893 
00894 Status impl::setMtu(int32_t mtu)
00895 {
00896     Status status = Status_Ok;
00897 
00898     //
00899     // Firmware v2.3 or better will send us an MTU-sized
00900     // response packet, which can be used to verify the
00901     // MTU setting before we actually make the change.
00902 
00903     if (m_sensorVersion.firmwareVersion <= 0x0202)
00904         status = waitAck(wire::SysMtu(mtu));
00905     else {
00906 
00907         wire::SysTestMtuResponse resp;
00908         status = waitData(wire::SysTestMtu(mtu), resp);
00909         if (Status_Ok == status)
00910             status = waitAck(wire::SysMtu(mtu));
00911     }
00912 
00913     if (Status_Ok == status)
00914         m_sensorMtu = mtu;
00915 
00916     return status;
00917 }
00918 
00919 Status impl::getMtu(int32_t& mtu)
00920 {
00921     wire::SysMtu resp;
00922 
00923     Status status = waitData(wire::SysGetMtu(), resp);
00924     if (Status_Ok == status)
00925         mtu = resp.mtu;
00926 
00927     return status;
00928 }
00929 
00930 //
00931 // Set/get the network configuration
00932 
00933 Status impl::setNetworkConfig(const system::NetworkConfig& c)
00934 {
00935     return waitAck(wire::SysNetwork(c.ipv4Address,
00936                                     c.ipv4Gateway,
00937                                     c.ipv4Netmask));
00938 }
00939 
00940 Status impl::getNetworkConfig(system::NetworkConfig& c)
00941 {
00942     wire::SysNetwork resp;
00943 
00944     Status status = waitData(wire::SysGetNetwork(), resp);
00945     if (Status_Ok == status) {
00946         c.ipv4Address = resp.address;
00947         c.ipv4Gateway = resp.gateway;
00948         c.ipv4Netmask = resp.netmask;
00949     }
00950 
00951     return status;
00952 }
00953 
00954 //
00955 // Get device info from sensor
00956 
00957 Status impl::getDeviceInfo(system::DeviceInfo& info)
00958 {
00959     wire::SysDeviceInfo w;
00960 
00961     Status status = waitData(wire::SysGetDeviceInfo(), w);
00962     if (Status_Ok != status)
00963         return status;
00964 
00965     info.name             = w.name;
00966     info.buildDate        = w.buildDate;
00967     info.serialNumber     = w.serialNumber;
00968     info.hardwareRevision = hardwareWireToApi(w.hardwareRevision);
00969     info.pcbs.clear();
00970 
00971     for(uint8_t i=0; i<w.numberOfPcbs; i++) {
00972         system::PcbInfo pcb;
00973 
00974         pcb.name     = w.pcbs[i].name;
00975         pcb.revision = w.pcbs[i].revision;
00976 
00977         info.pcbs.push_back(pcb);
00978     }
00979         
00980     info.imagerName              = w.imagerName;
00981     info.imagerType              = imagerWireToApi(w.imagerType);
00982     info.imagerWidth             = w.imagerWidth;
00983     info.imagerHeight            = w.imagerHeight;
00984     info.lensName                = w.lensName;
00985     info.lensType                = w.lensType;
00986     info.nominalBaseline         = w.nominalBaseline;
00987     info.nominalFocalLength      = w.nominalFocalLength;
00988     info.nominalRelativeAperture = w.nominalRelativeAperture;
00989     info.lightingType            = w.lightingType;
00990     info.numberOfLights          = w.numberOfLights;
00991     info.laserName               = w.laserName;
00992     info.laserType               = w.laserType;
00993     info.motorName               = w.motorName;
00994     info.motorType               = w.motorType;
00995     info.motorGearReduction      = w.motorGearReduction;
00996 
00997     return Status_Ok;
00998 }
00999 
01000 
01001 Status impl::getDeviceStatus(system::StatusMessage& status)
01002 {
01003     status.uptime = m_statusResponseMessage.uptime;
01004 
01005     status.systemOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_GENERAL_OK) ==
01006         wire::StatusResponse::STATUS_GENERAL_OK;
01007     status.laserOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_LASER_OK) ==
01008         wire::StatusResponse::STATUS_LASER_OK;
01009     status.laserMotorOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_LASER_MOTOR_OK) ==
01010         wire::StatusResponse::STATUS_LASER_MOTOR_OK;
01011     status.camerasOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_CAMERAS_OK) ==
01012         wire::StatusResponse::STATUS_CAMERAS_OK;
01013     status.imuOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_IMU_OK) ==
01014         wire::StatusResponse::STATUS_IMU_OK;
01015     status.externalLedsOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_EXTERNAL_LED_OK) ==
01016         wire::StatusResponse::STATUS_EXTERNAL_LED_OK;
01017     status.processingPipelineOk = (m_statusResponseMessage.status & wire::StatusResponse::STATUS_PIPELINE_OK) ==
01018         wire::StatusResponse::STATUS_PIPELINE_OK;
01019 
01020     status.powerSupplyTemperature = m_statusResponseMessage.temperature0;
01021     status.fpgaTemperature = m_statusResponseMessage.temperature1;
01022     status.leftImagerTemperature = m_statusResponseMessage.temperature2;
01023     status.rightImagerTemperature = m_statusResponseMessage.temperature3;
01024 
01025     status.inputVoltage = m_statusResponseMessage.inputVolts;
01026     status.inputCurrent = m_statusResponseMessage.inputCurrent;
01027     status.fpgaPower = m_statusResponseMessage.fpgaPower;
01028     status.logicPower = m_statusResponseMessage.logicPower;
01029     status.imagerPower = m_statusResponseMessage.imagerPower;
01030 
01031     return Status_Ok;
01032 }
01033 
01034 Status impl::getExternalCalibration(system::ExternalCalibration& calibration)
01035 {
01036     wire::SysExternalCalibration d;
01037 
01038     Status status = waitData(wire::SysGetExternalCalibration(), d);
01039     if (Status_Ok != status)
01040         return status;
01041 
01042     calibration.x = d.calibration[0];
01043     calibration.y = d.calibration[1];
01044     calibration.z = d.calibration[2];
01045     calibration.roll = d.calibration[3];
01046     calibration.pitch = d.calibration[4];
01047     calibration.yaw = d.calibration[5];
01048 
01049     return Status_Ok;
01050 }
01051 
01052 Status impl::setExternalCalibration(const system::ExternalCalibration& calibration)
01053 {
01054     wire::SysExternalCalibration w;
01055 
01056     w.calibration[0] = calibration.x;
01057     w.calibration[1] = calibration.y;
01058     w.calibration[2] = calibration.z;
01059     w.calibration[3] = calibration.roll;
01060     w.calibration[4] = calibration.pitch;
01061     w.calibration[5] = calibration.yaw;
01062 
01063     return waitAck(w);
01064 
01065     return Status_Ok;
01066 }
01067 
01068 //
01069 // Sets the device info
01070 
01071 Status impl::setDeviceInfo(const std::string& key,
01072                            const system::DeviceInfo& info)
01073 {
01074     wire::SysDeviceInfo w;
01075 
01076     w.key              = key; // must match device firmware key
01077     w.name             = info.name;
01078     w.buildDate        = info.buildDate;
01079     w.serialNumber     = info.serialNumber;
01080     w.hardwareRevision = hardwareApiToWire(info.hardwareRevision);
01081     w.numberOfPcbs     = std::min((uint32_t) info.pcbs.size(), 
01082                                   (uint32_t) wire::SysDeviceInfo::MAX_PCBS);
01083     for(uint32_t i=0; i<w.numberOfPcbs; i++) {
01084         w.pcbs[i].name     = info.pcbs[i].name;
01085         w.pcbs[i].revision = info.pcbs[i].revision;
01086     }
01087         
01088     w.imagerName              = info.imagerName;
01089     w.imagerType              = imagerApiToWire(info.imagerType);
01090     w.imagerWidth             = info.imagerWidth;
01091     w.imagerHeight            = info.imagerHeight;
01092     w.lensName                = info.lensName;
01093     w.lensType                = info.lensType;
01094     w.nominalBaseline         = info.nominalBaseline;
01095     w.nominalFocalLength      = info.nominalFocalLength;
01096     w.nominalRelativeAperture = info.nominalRelativeAperture;
01097     w.lightingType            = info.lightingType;
01098     w.numberOfLights          = info.numberOfLights;
01099     w.laserName               = info.laserName;
01100     w.laserType               = info.laserType;
01101     w.motorName               = info.motorName;
01102     w.motorType               = info.motorType;
01103     w.motorGearReduction      = info.motorGearReduction;
01104 
01105     return waitAck(w);
01106 }
01107 
01108 //
01109 // Flash the bitstream file (dangerous!)
01110 
01111 Status impl::flashBitstream(const std::string& filename)
01112 {
01113     return doFlashOp(filename,
01114                      wire::SysFlashOp::OP_PROGRAM,
01115                      wire::SysFlashOp::RGN_BITSTREAM);
01116 }
01117 
01118 //
01119 // Flash the firmware file (dangerous!)
01120 
01121 Status impl::flashFirmware(const std::string& filename)
01122 {
01123     return doFlashOp(filename,
01124                      wire::SysFlashOp::OP_PROGRAM,
01125                      wire::SysFlashOp::RGN_FIRMWARE);
01126 }
01127 
01128 //
01129 // Verify the bitstream file
01130 
01131 Status impl::verifyBitstream(const std::string& filename)
01132 {
01133     return doFlashOp(filename,
01134                      wire::SysFlashOp::OP_VERIFY,
01135                      wire::SysFlashOp::RGN_BITSTREAM);
01136 }
01137 
01138 //
01139 // Verify the firmware file
01140 
01141 Status impl::verifyFirmware(const std::string& filename)
01142 {
01143     return doFlashOp(filename,
01144                      wire::SysFlashOp::OP_VERIFY,
01145                      wire::SysFlashOp::RGN_FIRMWARE);
01146 }
01147 
01148 //
01149 // Get IMU information
01150 
01151 Status impl::getImuInfo(uint32_t& maxSamplesPerMessage,
01152                         std::vector<imu::Info>& info)
01153 {
01154     wire::ImuInfo w;
01155 
01156     Status status = waitData(wire::ImuGetInfo(), w);
01157     if (Status_Ok != status)
01158         return status;
01159 
01160     //
01161     // Wire --> API
01162 
01163     maxSamplesPerMessage = w.maxSamplesPerMessage;
01164     info.resize(w.details.size());
01165     for(uint32_t i=0; i<w.details.size(); i++) {
01166 
01167         const wire::imu::Details& d = w.details[i];
01168 
01169         info[i].name   = d.name;
01170         info[i].device = d.device;
01171         info[i].units  = d.units;
01172         
01173         info[i].rates.resize(d.rates.size());
01174         for(uint32_t j=0; j<d.rates.size(); j++) {
01175             info[i].rates[j].sampleRate      = d.rates[j].sampleRate;
01176             info[i].rates[j].bandwidthCutoff = d.rates[j].bandwidthCutoff;
01177         }
01178         info[i].ranges.resize(d.ranges.size());
01179         for(uint32_t j=0; j<d.ranges.size(); j++) {
01180             info[i].ranges[j].range      = d.ranges[j].range;
01181             info[i].ranges[j].resolution = d.ranges[j].resolution;
01182         }
01183     }
01184 
01185     return Status_Ok;
01186 }
01187 
01188 //
01189 // Get IMU configuration
01190 
01191 Status impl::getImuConfig(uint32_t& samplesPerMessage,
01192                           std::vector<imu::Config>& c)
01193 {
01194     wire::ImuConfig w;
01195 
01196     Status status = waitData(wire::ImuGetConfig(), w);
01197     if (Status_Ok != status)
01198         return status;
01199 
01200     //
01201     // Wire --> API
01202 
01203     samplesPerMessage = w.samplesPerMessage;
01204     c.resize(w.configs.size());
01205     for(uint32_t i=0; i<w.configs.size(); i++) {
01206         c[i].name            = w.configs[i].name;
01207         c[i].enabled         = (w.configs[i].flags & wire::imu::Config::FLAGS_ENABLED);
01208         c[i].rateTableIndex  = w.configs[i].rateTableIndex;
01209         c[i].rangeTableIndex = w.configs[i].rangeTableIndex;
01210     }
01211 
01212     return Status_Ok;
01213 }
01214 
01215 //
01216 // Set IMU configuration
01217 
01218 Status impl::setImuConfig(bool storeSettingsInFlash,
01219                           uint32_t samplesPerMessage,
01220                           const std::vector<imu::Config>& c)
01221 {
01222     wire::ImuConfig w;
01223 
01224     //
01225     // API --> wire
01226 
01227     w.storeSettingsInFlash = storeSettingsInFlash ? 1 : 0;
01228     w.samplesPerMessage    = samplesPerMessage;
01229     w.configs.resize(c.size());
01230     for(uint32_t i=0; i<c.size(); i++) {
01231         w.configs[i].name            = c[i].name;
01232         w.configs[i].flags           = c[i].enabled ? wire::imu::Config::FLAGS_ENABLED : 0;
01233         w.configs[i].rateTableIndex  = c[i].rateTableIndex;
01234         w.configs[i].rangeTableIndex = c[i].rangeTableIndex;
01235     }
01236 
01237     return waitAck(w);
01238 }
01239 
01240 //
01241 // Get recommended large buffer pool count/size
01242 
01243 Status impl::getLargeBufferDetails(uint32_t& bufferCount,
01244                                    uint32_t& bufferSize)
01245 {
01246     bufferCount = RX_POOL_LARGE_BUFFER_COUNT;
01247     bufferSize  = RX_POOL_LARGE_BUFFER_SIZE;
01248     
01249     return Status_Ok;
01250 }
01251 
01252 //
01253 // Replace internal large buffers with user supplied
01254 
01255 Status impl::setLargeBuffers(const std::vector<uint8_t*>& buffers,
01256                              uint32_t                     bufferSize)
01257 {
01258     if (buffers.size() < RX_POOL_LARGE_BUFFER_COUNT)
01259         CRL_DEBUG("WARNING: supplying less than recommended number of large buffers: %ld/%ld\n",
01260                   static_cast<long int>(buffers.size()), 
01261                   static_cast<long int>(RX_POOL_LARGE_BUFFER_COUNT));
01262     if (bufferSize < RX_POOL_LARGE_BUFFER_SIZE)
01263         CRL_DEBUG("WARNING: supplying smaller than recommended large buffers: %ld/%ld bytes\n",
01264                   static_cast<long int>(bufferSize), 
01265                   static_cast<long int>(RX_POOL_LARGE_BUFFER_SIZE));
01266 
01267     try {
01268 
01269         utility::ScopedLock lock(m_rxLock); // halt potential pool traversal
01270         
01271         //
01272         // Deletion is safe even if the buffer is in use elsewhere
01273         // (BufferStream is reference counted.)
01274 
01275         BufferPool::const_iterator it;
01276         for(it  = m_rxLargeBufferPool.begin();
01277             it != m_rxLargeBufferPool.end();
01278             ++it)
01279             delete *it;
01280 
01281         m_rxLargeBufferPool.clear();
01282 
01283         for(uint32_t i=0; i<buffers.size(); i++)
01284             m_rxLargeBufferPool.push_back(new utility::BufferStreamWriter(buffers[i], bufferSize));
01285 
01286     } catch (const std::exception& e) {
01287         CRL_DEBUG("exception: %s\n", e.what());
01288         return Status_Exception;
01289     }
01290 
01291     return Status_Ok;
01292 }
01293 
01294 //
01295 // Retrieve the system-assigned local UDP port
01296 
01297 Status impl::getLocalUdpPort(uint16_t& port)
01298 {
01299     port = m_serverSocketPort;
01300     return Status_Ok;
01301 }
01302 }}}; // namespaces


multisense_lib
Author(s):
autogenerated on Fri Apr 5 2019 02:28:24