00001
00002
00003 #include <iomanip>
00004
00005 #include "hw-monitor.h"
00006 #include "ds-private.h"
00007
00008 #pragma pack(push, 1) // All structs in this file are byte-aligned
00009
00010 enum class command : uint32_t
00011 {
00012 peek = 0x11,
00013 poke = 0x12,
00014 download_spi_flash = 0x1A,
00015 get_fwrevision = 0x21
00016 };
00017
00018 enum class command_modifier : uint32_t { direct = 0x10 };
00019
00020 #define SPI_FLASH_PAGE_SIZE_IN_BYTES 0x100
00021 #define SPI_FLASH_SECTOR_SIZE_IN_BYTES 0x1000
00022 #define SPI_FLASH_SIZE_IN_SECTORS 256
00023 #define SPI_FLASH_TOTAL_SIZE_IN_BYTES (SPI_FLASH_SIZE_IN_SECTORS * SPI_FLASH_SECTOR_SIZE_IN_BYTES)
00024 #define SPI_FLASH_PAGES_PER_SECTOR (SPI_FLASH_SECTOR_SIZE_IN_BYTES / SPI_FLASH_PAGE_SIZE_IN_BYTES)
00025 #define SPI_FLASH_SECTORS_RESERVED_FOR_FIRMWARE 160
00026 #define NV_NON_FIRMWARE_START (SPI_FLASH_SECTORS_RESERVED_FOR_FIRMWARE * SPI_FLASH_SECTOR_SIZE_IN_BYTES)
00027 #define NV_ADMIN_DATA_N_ENTRIES 9
00028 #define NV_CALIBRATION_DATA_ADDRESS_INDEX 0
00029 #define NV_NON_FIRMWARE_ROOT_ADDRESS NV_NON_FIRMWARE_START
00030 #define CAM_INFO_BLOCK_LEN 2048
00031
00032 namespace rsimpl {
00033 namespace ds
00034 {
00035
00036 void xu_read(const uvc::device & device, uvc::extension_unit xu, control xu_ctrl, void * buffer, uint32_t length)
00037 {
00038 uvc::get_control_with_retry(device, xu, static_cast<int>(xu_ctrl), buffer, length);
00039 }
00040
00041 void xu_write(uvc::device & device, uvc::extension_unit xu, control xu_ctrl, void * buffer, uint32_t length)
00042 {
00043 uvc::set_control_with_retry(device, xu, static_cast<int>(xu_ctrl), buffer, length);
00044 }
00045
00046 struct CommandResponsePacket
00047 {
00048 command code; command_modifier modifier;
00049 uint32_t tag, address, value, reserved[59];
00050 CommandResponsePacket() { std::memset(this, 0, sizeof(CommandResponsePacket)); }
00051 CommandResponsePacket(command code, uint32_t address = 0, uint32_t value = 0) : code(code), modifier(command_modifier::direct), tag(12), address(address), value(value)
00052 {
00053 std::memset(reserved, 0, sizeof(reserved));
00054 }
00055 };
00056
00057 CommandResponsePacket send_command_and_receive_response(uvc::device & device, const CommandResponsePacket & command)
00058 {
00059 CommandResponsePacket c = command, r;
00060 set_control(device, lr_xu, static_cast<int>(control::command_response), &c, sizeof(c));
00061 get_control(device, lr_xu, static_cast<int>(control::command_response), &r, sizeof(r));
00062 return r;
00063 }
00064
00065 void bulk_usb_command(uvc::device & device, std::timed_mutex & mutex, unsigned char out_ep, uint8_t *out, size_t outSize, uint32_t & op, unsigned char in_ep, uint8_t * in, size_t & inSize, int timeout)
00066 {
00067
00068 errno = 0;
00069
00070 int outXfer;
00071
00072 if (!mutex.try_lock_for(std::chrono::milliseconds(timeout))) throw std::runtime_error("timed_mutex::try_lock_for(...) timed out");
00073 std::lock_guard<std::timed_mutex> guard(mutex, std::adopt_lock);
00074
00075 bulk_transfer(device, out_ep, out, (int)outSize, &outXfer, timeout);
00076
00077
00078 if (in && inSize)
00079 {
00080 uint8_t buf[1024];
00081
00082 errno = 0;
00083
00084 bulk_transfer(device, in_ep, buf, sizeof(buf), &outXfer, timeout);
00085 if (outXfer < (int)sizeof(uint32_t)) throw std::runtime_error("incomplete bulk usb transfer");
00086
00087 op = *(uint32_t *)buf;
00088 if (outXfer > (int)inSize) throw std::runtime_error("bulk transfer failed - user buffer too small");
00089 inSize = outXfer;
00090 memcpy(in, buf, inSize);
00091 }
00092 }
00093
00094 bool read_device_pages(uvc::device & dev, uint32_t address, unsigned char * buffer, uint32_t nPages)
00095 {
00096 int addressTest = SPI_FLASH_TOTAL_SIZE_IN_BYTES - address - nPages * SPI_FLASH_PAGE_SIZE_IN_BYTES;
00097
00098 if (!nPages || addressTest < 0)
00099 return false;
00100
00101
00102
00103
00104
00105
00106
00107 send_command_and_receive_response(dev, CommandResponsePacket(command::download_spi_flash, address, nPages * SPI_FLASH_PAGE_SIZE_IN_BYTES));
00108
00109 uint8_t *p = buffer;
00110 uint16_t spiLength = SPI_FLASH_PAGE_SIZE_IN_BYTES;
00111 for (unsigned int i = 0; i < nPages; ++i)
00112 {
00113 xu_read(dev, lr_xu, control::command_response, p, spiLength);
00114 p += SPI_FLASH_PAGE_SIZE_IN_BYTES;
00115 }
00116 return true;
00117 }
00118
00119 void read_arbitrary_chunk(uvc::device & dev, uint32_t address, void * dataIn, int lengthInBytesIn)
00120 {
00121 unsigned char * data = (unsigned char *)dataIn;
00122 int lengthInBytes = lengthInBytesIn;
00123 unsigned char page[SPI_FLASH_PAGE_SIZE_IN_BYTES];
00124 int nPagesToRead;
00125 uint32_t startAddress = address;
00126 if (startAddress & 0xff)
00127 {
00128
00129 startAddress = startAddress & ~0xff;
00130 uint32_t startInPage = address - startAddress;
00131 uint32_t lengthToCopy = SPI_FLASH_PAGE_SIZE_IN_BYTES - startInPage;
00132 if (lengthToCopy > (uint32_t)lengthInBytes)
00133 lengthToCopy = lengthInBytes;
00134 read_device_pages(dev, startAddress, page, 1);
00135 memcpy(data, page + startInPage, lengthToCopy);
00136 lengthInBytes -= lengthToCopy;
00137 data += lengthToCopy;
00138 startAddress += SPI_FLASH_PAGE_SIZE_IN_BYTES;
00139 }
00140
00141 nPagesToRead = lengthInBytes / SPI_FLASH_PAGE_SIZE_IN_BYTES;
00142
00143 if (nPagesToRead > 0)
00144 read_device_pages(dev, startAddress, data, nPagesToRead);
00145
00146 lengthInBytes -= (nPagesToRead * SPI_FLASH_PAGE_SIZE_IN_BYTES);
00147
00148 if (lengthInBytes)
00149 {
00150
00151 data += (nPagesToRead * SPI_FLASH_PAGE_SIZE_IN_BYTES);
00152 startAddress += (nPagesToRead * SPI_FLASH_PAGE_SIZE_IN_BYTES);
00153 read_device_pages(dev, startAddress, page, 1);
00154 memcpy(data, page, lengthInBytes);
00155 }
00156 }
00157
00158 bool read_admin_sector(uvc::device & dev, unsigned char data[SPI_FLASH_SECTOR_SIZE_IN_BYTES], int whichAdminSector)
00159 {
00160 uint32_t adminSectorAddresses[NV_ADMIN_DATA_N_ENTRIES];
00161
00162 read_arbitrary_chunk(dev, NV_NON_FIRMWARE_ROOT_ADDRESS, adminSectorAddresses, NV_ADMIN_DATA_N_ENTRIES * sizeof(adminSectorAddresses[0]));
00163
00164 if (whichAdminSector >= 0 && whichAdminSector < NV_ADMIN_DATA_N_ENTRIES)
00165 {
00166 uint32_t pageAddressInBytes = adminSectorAddresses[whichAdminSector];
00167 return read_device_pages(dev, pageAddressInBytes, data, SPI_FLASH_PAGES_PER_SECTOR);
00168 }
00169
00170 return false;
00171 }
00172
00173 ds_calibration read_calibration_and_rectification_parameters(const uint8_t(&flash_data_buffer)[SPI_FLASH_SECTOR_SIZE_IN_BYTES])
00174 {
00175 struct RectifiedIntrinsics
00176 {
00177 big_endian<float> rfx, rfy;
00178 big_endian<float> rpx, rpy;
00179 big_endian<uint32_t> rw, rh;
00180 operator rs_intrinsics () const { return{ (int)rw, (int)rh, rpx, rpy, rfx, rfy, RS_DISTORTION_NONE, {0,0,0,0,0} }; }
00181 };
00182
00183 ds_calibration cameraCalib = {};
00184 cameraCalib.version = reinterpret_cast<const big_endian<uint32_t> &>(flash_data_buffer);
00185 if (cameraCalib.version == 0)
00186 {
00187 struct UnrectifiedIntrinsicsV0
00188 {
00189 big_endian<float> fx, fy;
00190 big_endian<float> px, py;
00191 big_endian<double> k[5];
00192 big_endian<uint32_t> w, h;
00193 operator rs_intrinsics () const { return{ (int)w, (int)h, px, py, fx, fy, RS_DISTORTION_MODIFIED_BROWN_CONRADY, {(float)k[0],(float)k[1],(float)k[2],(float)k[3],(float)k[4]} }; }
00194 };
00195
00196 struct CameraCalibrationParametersV0
00197 {
00198 enum { MAX_INTRIN_RIGHT = 2 };
00199 enum { MAX_INTRIN_THIRD = 3 };
00200 enum { MAX_MODES_LR = 4 };
00201 enum { MAX_MODES_THIRD = 4 };
00202
00203 big_endian<uint32_t> versionNumber;
00204 big_endian<uint16_t> numIntrinsicsRight;
00205 big_endian<uint16_t> numIntrinsicsThird;
00206 big_endian<uint16_t> numRectifiedModesLR;
00207 big_endian<uint16_t> numRectifiedModesThird;
00208
00209 UnrectifiedIntrinsicsV0 intrinsicsLeft;
00210 UnrectifiedIntrinsicsV0 intrinsicsRight[MAX_INTRIN_RIGHT];
00211 UnrectifiedIntrinsicsV0 intrinsicsThird[MAX_INTRIN_THIRD];
00212
00213 RectifiedIntrinsics modesLR[MAX_INTRIN_RIGHT][MAX_MODES_LR];
00214 RectifiedIntrinsics modesThird[MAX_INTRIN_RIGHT][MAX_INTRIN_THIRD][MAX_MODES_THIRD];
00215
00216 big_endian<double> Rleft[MAX_INTRIN_RIGHT][9];
00217 big_endian<double> Rright[MAX_INTRIN_RIGHT][9];
00218 big_endian<double> Rthird[MAX_INTRIN_RIGHT][9];
00219
00220 big_endian<float> B[MAX_INTRIN_RIGHT];
00221 big_endian<float> T[MAX_INTRIN_RIGHT][3];
00222
00223 big_endian<double> Rworld[9];
00224 big_endian<float> Tworld[3];
00225 };
00226
00227 const auto & calib = reinterpret_cast<const CameraCalibrationParametersV0 &>(flash_data_buffer);
00228 for (int i = 0; i < 3; ++i) cameraCalib.modesLR[i] = calib.modesLR[0][i];
00229 for (int i = 0; i < 2; ++i)
00230 {
00231 cameraCalib.intrinsicsThird[i] = calib.intrinsicsThird[i];
00232 for (int j = 0; j < 2; ++j) cameraCalib.modesThird[i][j] = calib.modesThird[0][i][j];
00233 }
00234 for (int i = 0; i < 9; ++i) cameraCalib.Rthird[i] = static_cast<float>(calib.Rthird[0][i]);
00235 for (int i = 0; i < 3; ++i) cameraCalib.T[i] = calib.T[0][i];
00236 cameraCalib.B = calib.B[0];
00237 }
00238 else if (cameraCalib.version == 1 || cameraCalib.version == 2)
00239 {
00240 struct UnrectifiedIntrinsicsV2
00241 {
00242 big_endian<float> fx, fy;
00243 big_endian<float> px, py;
00244 big_endian<float> k[5];
00245 big_endian<uint32_t> w, h;
00246 operator rs_intrinsics () const { return{ (int)w, (int)h, px, py, fx, fy, RS_DISTORTION_MODIFIED_BROWN_CONRADY, {k[0],k[1],k[2],k[3],k[4]} }; }
00247 };
00248
00249 struct CameraCalibrationParametersV2
00250 {
00251 enum { MAX_INTRIN_RIGHT = 2 };
00252 enum { MAX_INTRIN_THIRD = 3 };
00253 enum { MAX_INTRIN_PLATFORM = 4 };
00254 enum { MAX_MODES_LR = 4 };
00255 enum { MAX_MODES_THIRD = 3 };
00256 enum { MAX_MODES_PLATFORM = 1 };
00257
00258 big_endian<uint32_t> versionNumber;
00259 big_endian<uint16_t> numIntrinsicsRight;
00260 big_endian<uint16_t> numIntrinsicsThird;
00261 big_endian<uint16_t> numIntrinsicsPlatform;
00262 big_endian<uint16_t> numRectifiedModesLR;
00263 big_endian<uint16_t> numRectifiedModesThird;
00264 big_endian<uint16_t> numRectifiedModesPlatform;
00265
00266 UnrectifiedIntrinsicsV2 intrinsicsLeft;
00267 UnrectifiedIntrinsicsV2 intrinsicsRight[MAX_INTRIN_RIGHT];
00268 UnrectifiedIntrinsicsV2 intrinsicsThird[MAX_INTRIN_THIRD];
00269 UnrectifiedIntrinsicsV2 intrinsicsPlatform[MAX_INTRIN_PLATFORM];
00270
00271 RectifiedIntrinsics modesLR[MAX_INTRIN_RIGHT][MAX_MODES_LR];
00272 RectifiedIntrinsics modesThird[MAX_INTRIN_RIGHT][MAX_INTRIN_THIRD][MAX_MODES_THIRD];
00273 RectifiedIntrinsics modesPlatform[MAX_INTRIN_RIGHT][MAX_INTRIN_PLATFORM][MAX_MODES_PLATFORM];
00274
00275 big_endian<float> Rleft[MAX_INTRIN_RIGHT][9];
00276 big_endian<float> Rright[MAX_INTRIN_RIGHT][9];
00277 big_endian<float> Rthird[MAX_INTRIN_RIGHT][9];
00278 big_endian<float> Rplatform[MAX_INTRIN_RIGHT][9];
00279
00280 big_endian<float> B[MAX_INTRIN_RIGHT];
00281 big_endian<float> T[MAX_INTRIN_RIGHT][3];
00282 big_endian<float> Tplatform[MAX_INTRIN_RIGHT][3];
00283
00284 big_endian<float> Rworld[9];
00285 big_endian<float> Tworld[3];
00286 };
00287
00288 const auto & calib = reinterpret_cast<const CameraCalibrationParametersV2 &>(flash_data_buffer);
00289 for (int i = 0; i < 3; ++i) cameraCalib.modesLR[i] = calib.modesLR[0][i];
00290 for (int i = 0; i < 2; ++i)
00291 {
00292 cameraCalib.intrinsicsThird[i] = calib.intrinsicsThird[i];
00293 for (int j = 0; j < 2; ++j) cameraCalib.modesThird[i][j] = calib.modesThird[0][i][j];
00294 }
00295 for (int i = 0; i < 9; ++i) cameraCalib.Rthird[i] = calib.Rthird[0][i];
00296 for (int i = 0; i < 3; ++i) cameraCalib.T[i] = calib.T[0][i];
00297 cameraCalib.B = calib.B[0];
00298 }
00299 else
00300 {
00301 throw std::runtime_error(to_string() << "Unsupported calibration version: " << cameraCalib.version);
00302 }
00303
00304 return cameraCalib;
00305 }
00306
00307 ds_head_content read_camera_head_contents(const uint8_t(&flash_data_buffer)[SPI_FLASH_SECTOR_SIZE_IN_BYTES], uint32_t & serial_number)
00308 {
00309
00310 ds_head_content head_content = reinterpret_cast<const ds_head_content &>(flash_data_buffer[CAM_INFO_BLOCK_LEN]);
00311
00312 serial_number = head_content.serial_number;
00313
00314 LOG_INFO("Serial number = " << head_content.serial_number);
00315 LOG_INFO("Model number = " << head_content.imager_model_number);
00316 LOG_INFO("Revision number = " << head_content.module_revision_number);
00317 LOG_INFO("Camera head contents version = " << head_content.camera_head_contents_version);
00318 if (head_content.camera_head_contents_version != ds_head_content::DS_HEADER_VERSION_NUMBER) LOG_WARNING("Camera head contents version != 12, data may be missing/incorrect");
00319 LOG_INFO("Module version = " << (int)head_content.module_version << "." << (int)head_content.module_major_version << "." << (int)head_content.module_minor_version << "." << (int)head_content.module_skew_version);
00320 LOG_INFO("OEM ID = " << head_content.oem_id);
00321 LOG_INFO("Lens type for left/right imagers = " << head_content.lens_type);
00322 LOG_INFO("Lens type for third imager = " << head_content.lens_type_third_imager);
00323 LOG_INFO("Lens coating for left/right imagers = " << head_content.lens_coating_type);
00324 LOG_INFO("Lens coating for third imager = " << head_content.lens_coating_type_third_imager);
00325 LOG_INFO("Nominal baseline (left to right) = " << head_content.nominal_baseline << " mm");
00326 LOG_INFO("Nominal baseline (left to third) = " << head_content.nominal_baseline_third_imager << " mm");
00327 LOG_INFO("Built on " << time_to_string(head_content.build_date) << " UTC");
00328 LOG_INFO("Calibrated on " << time_to_string(head_content.calibration_date) << " UTC");
00329 return head_content;
00330 }
00331
00332 ds_info read_camera_info(uvc::device & device)
00333 {
00334 uint8_t flashDataBuffer[SPI_FLASH_SECTOR_SIZE_IN_BYTES];
00335 if (!read_admin_sector(device, flashDataBuffer, NV_CALIBRATION_DATA_ADDRESS_INDEX)) throw std::runtime_error("Could not read calibration sector");
00336
00337 ds_info cam_info = {};
00338
00339 try
00340 {
00341 cam_info.calibration = read_calibration_and_rectification_parameters(flashDataBuffer);
00342 cam_info.head_content = read_camera_head_contents(flashDataBuffer, cam_info.calibration.serial_number);
00343 }
00344 catch (std::runtime_error &err){ LOG_ERROR("Failed to read camera info " << err.what()); }
00345 catch (...){ LOG_ERROR("Failed to read camera info, may not work correctly"); }
00346
00347 return cam_info;
00348 }
00349
00350 std::string read_firmware_version(uvc::device & device)
00351 {
00352 auto response = send_command_and_receive_response(device, CommandResponsePacket(command::get_fwrevision));
00353 return reinterpret_cast<const char *>(response.reserved);
00354 }
00355
00356 std::string read_isp_firmware_version(uvc::device & device)
00357 {
00358 auto response = send_command_and_receive_response(device, CommandResponsePacket(command::get_fwrevision));
00359 return to_string() << "0x" << std::hex << response.reserved[4];
00360 }
00361
00362 void set_stream_intent(uvc::device & device, uint8_t & intent)
00363 {
00364 xu_write(device, lr_xu, control::stream_intent, intent);
00365 }
00366
00367 void get_stream_status(const uvc::device & device, int & status)
00368 {
00369 uint8_t s[4] = { 255, 255, 255, 255 };
00370 xu_read(device, lr_xu, control::status, s, sizeof(uint32_t));
00371 status = rsimpl::pack(s[0], s[1], s[2], s[3]);
00372 }
00373
00374 void force_firmware_reset(uvc::device & device)
00375 {
00376 try
00377 {
00378 uint8_t reset = 1;
00379 xu_write(device, lr_xu, control::sw_reset, &reset, sizeof(uint8_t));
00380 }
00381 catch (...) {}
00382 }
00383
00384 bool get_emitter_state(const uvc::device & device, bool is_streaming, bool is_depth_enabled)
00385 {
00386 auto byte = xu_read<uint8_t>(device, lr_xu, control::emitter);
00387 if (is_streaming) return (byte & 1 ? true : false);
00388 else if (byte & 4) return (byte & 2 ? true : false);
00389 else return is_depth_enabled;
00390 }
00391
00392 void set_emitter_state(uvc::device & device, bool state)
00393 {
00394 xu_write(device, lr_xu, control::emitter, uint8_t(state ? 1 : 0));
00395 }
00396
00397 void get_register_value(uvc::device & device, uint32_t reg, uint32_t & value)
00398 {
00399 value = send_command_and_receive_response(device, CommandResponsePacket(command::peek, reg)).value;
00400 }
00401 void set_register_value(uvc::device & device, uint32_t reg, uint32_t value)
00402 {
00403 send_command_and_receive_response(device, CommandResponsePacket(command::poke, reg, value));
00404 }
00405
00406 const dc_params dc_params::presets[] = {
00407 {5, 5, 192, 1, 512, 6, 24, 27, 7, 24},
00408 {5, 5, 0, 0, 1023, 0, 0, 0, 0, 2047},
00409 {5, 5, 115, 1, 512, 6, 18, 25, 3, 24},
00410 {5, 5, 185, 5, 505, 6, 35, 45, 45, 14},
00411 {5, 5, 175, 24, 430, 6, 48, 47, 24, 12},
00412 {5, 5, 235, 27, 420, 8, 80, 70, 90, 12},
00413 };
00414
00415 }
00416
00417 namespace zr300
00418 {
00419
00420
00421 const uvc::guid MOTION_MODULE_USB_DEVICE_GUID = { 0xC0B55A29, 0xD7B6, 0x436E,{ 0xA6, 0xEF, 0x2E, 0x76, 0xED, 0x0A, 0xBC, 0xA5 } };
00422 const unsigned short motion_module_interrupt_interface = 0x2;
00423
00424 uint8_t get_fisheye_external_trigger(const uvc::device & device)
00425 {
00426 return ds::xu_read<uint8_t>(device, fisheye_xu, ds::control::fisheye_xu_ext_trig);
00427 }
00428
00429 void set_fisheye_external_trigger(uvc::device & device, uint8_t ext_trig)
00430 {
00431 ds::xu_write(device, fisheye_xu, ds::control::fisheye_xu_ext_trig, &ext_trig, sizeof(ext_trig));
00432 }
00433
00434 uint8_t get_fisheye_strobe(const uvc::device & device)
00435 {
00436 return ds::xu_read<uint8_t>(device, fisheye_xu, ds::control::fisheye_xu_strobe);
00437 }
00438
00439 void set_fisheye_strobe(uvc::device & device, uint8_t strobe)
00440 {
00441 ds::xu_write(device, fisheye_xu, ds::control::fisheye_xu_strobe, &strobe, sizeof(strobe));
00442 }
00443
00444 uint16_t get_fisheye_exposure(const uvc::device & device)
00445 {
00446 return ds::xu_read<uint16_t>(device, fisheye_xu, ds::control::fisheye_exposure);
00447 }
00448
00449 void set_fisheye_exposure(uvc::device & device, uint16_t exposure)
00450 {
00451 ds::xu_write(device, fisheye_xu, ds::control::fisheye_exposure, &exposure, sizeof(exposure));
00452 }
00453
00454 void claim_motion_module_interface(uvc::device & device)
00455 {
00456 claim_aux_interface(device, MOTION_MODULE_USB_DEVICE_GUID, motion_module_interrupt_interface);
00457 }
00458
00459 }
00460 }
00461
00462 #pragma pack(pop)