| #include "USBCamera.h" |
| |
| #include "Utility.h" |
| |
| #include <regex> |
| #include <chrono> |
| #include <thread> |
| #include <memory> |
| #include <iostream> |
| #include <iomanip> |
| |
| // This macro expands the given imaq function to ensure that it is called and |
| // properly checked for an error, calling the wpi_setImaqErrorWithContext |
| // macro |
| // To call it, just give the name of the function and the arguments |
| #define SAFE_IMAQ_CALL(funName, ...) \ |
| { \ |
| unsigned int error = funName(__VA_ARGS__); \ |
| if (error != IMAQdxErrorSuccess) \ |
| wpi_setImaqErrorWithContext(error, #funName); \ |
| } |
| |
| /** |
| * Helper function to determine the size of a jpeg. The general structure of |
| * how to parse a jpeg for length can be found in this stackoverflow article: |
| * http://stackoverflow.com/a/1602428. Be sure to also read the comments for |
| * the SOS flag explanation. |
| */ |
| unsigned int USBCamera::GetJpegSize(void* buffer, unsigned int buffSize) { |
| uint8_t* data = (uint8_t*)buffer; |
| if (!wpi_assert(data[0] == 0xff && data[1] == 0xd8)) return 0; |
| unsigned int pos = 2; |
| while (pos < buffSize) { |
| // All control markers start with 0xff, so if this isn't present, |
| // the JPEG is not valid |
| if (!wpi_assert(data[pos] == 0xff)) return 0; |
| unsigned char t = data[pos + 1]; |
| // These are RST markers. We just skip them and move onto the next marker |
| if (t == 0x01 || (t >= 0xd0 && t <= 0xd7)) { |
| pos += 2; |
| } else if (t == 0xd9) { |
| // End of Image, add 2 for this and 0-indexed |
| return pos + 2; |
| } else if (!wpi_assert(t != 0xd8)) { |
| // Another start of image, invalid image |
| return 0; |
| } else if (t == 0xda) { |
| // SOS marker. The next two bytes are a 16-bit big-endian int that is |
| // the length of the SOS header, skip that |
| unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 | |
| ((unsigned int)data[pos + 3] & 0xff)); |
| pos += len + 2; |
| // The next marker is the first marker that is 0xff followed by a non-RST |
| // element. 0xff followed by 0x00 is an escaped 0xff. 0xd0-0xd7 are RST |
| // markers |
| while (data[pos] != 0xff || data[pos + 1] == 0x00 || |
| (data[pos + 1] >= 0xd0 && data[pos + 1] <= 0xd7)) { |
| pos += 1; |
| if (pos >= buffSize) return 0; |
| } |
| } else { |
| // This is one of several possible markers. The next two bytes are a |
| // 16-bit |
| // big-endian int with the length of the marker header, skip that then |
| // continue searching |
| unsigned int len = (((unsigned int)(data[pos + 2] & 0xff)) << 8 | |
| ((unsigned int)data[pos + 3] & 0xff)); |
| pos += len + 2; |
| } |
| } |
| |
| return 0; |
| } |
| |
| USBCamera::USBCamera(std::string name, bool useJpeg) |
| : m_name(name), |
| m_useJpeg(useJpeg) {} |
| |
| void USBCamera::OpenCamera() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| for (unsigned int i = 0; i < 3; i++) { |
| uInt32 id = 0; |
| // Can't use SAFE_IMAQ_CALL here because we only error on the third time |
| IMAQdxError error = IMAQdxOpenCamera( |
| m_name.c_str(), IMAQdxCameraControlModeController, &id); |
| if (error != IMAQdxErrorSuccess) { |
| // Only error on the 3rd try |
| if (i >= 2) wpi_setImaqErrorWithContext(error, "IMAQdxOpenCamera"); |
| // Sleep for a few seconds to ensure the error has been dealt with |
| std::this_thread::sleep_for(std::chrono::milliseconds(2000)); |
| } else { |
| m_id = id; |
| m_open = true; |
| return; |
| } |
| } |
| } |
| |
| void USBCamera::CloseCamera() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (!m_open) return; |
| SAFE_IMAQ_CALL(IMAQdxCloseCamera, m_id); |
| m_id = 0; |
| m_open = false; |
| } |
| |
| void USBCamera::StartCapture() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (!m_open || m_active) return; |
| SAFE_IMAQ_CALL(IMAQdxConfigureGrab, m_id); |
| SAFE_IMAQ_CALL(IMAQdxStartAcquisition, m_id); |
| m_active = true; |
| } |
| |
| void USBCamera::StopCapture() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (!m_open || !m_active) return; |
| SAFE_IMAQ_CALL(IMAQdxStopAcquisition, m_id); |
| SAFE_IMAQ_CALL(IMAQdxUnconfigureAcquisition, m_id); |
| m_active = false; |
| } |
| |
| void USBCamera::UpdateSettings() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| bool wasActive = m_active; |
| |
| if (wasActive) StopCapture(); |
| if (m_open) CloseCamera(); |
| OpenCamera(); |
| |
| uInt32 count = 0; |
| uInt32 currentMode = 0; |
| SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, nullptr, &count, ¤tMode); |
| auto modes = std::make_unique<IMAQdxVideoMode[]>(count); |
| SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes.get(), &count, ¤tMode); |
| |
| // Groups are: |
| // 0 - width |
| // 1 - height |
| // 2 - format |
| // 3 - fps |
| std::regex reMode("([0-9]+)\\s*x\\s*([0-9]+)\\s+(.*?)\\s+([0-9.]+)\\s*fps"); |
| IMAQdxVideoMode* foundMode = nullptr; |
| IMAQdxVideoMode* currentModePtr = &modes[currentMode]; |
| double foundFps = 1000.0; |
| |
| // Loop through the modes, and find the match with the lowest fps |
| for (unsigned int i = 0; i < count; i++) { |
| std::cmatch m; |
| if (!std::regex_match(modes[i].Name, m, reMode)) continue; |
| unsigned int width = (unsigned int)std::stoul(m[1].str()); |
| unsigned int height = (unsigned int)std::stoul(m[2].str()); |
| if (width != m_width) continue; |
| if (height != m_height) continue; |
| double fps = atof(m[4].str().c_str()); |
| if (fps < m_fps) continue; |
| if (fps > foundFps) continue; |
| bool isJpeg = |
| m[3].str().compare("jpeg") == 0 || m[3].str().compare("JPEG") == 0; |
| if ((m_useJpeg && !isJpeg) || (!m_useJpeg && isJpeg)) continue; |
| foundMode = &modes[i]; |
| foundFps = fps; |
| } |
| if (foundMode != nullptr) { |
| if (foundMode->Value != currentModePtr->Value) { |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, IMAQdxAttributeVideoMode, |
| IMAQdxValueTypeU32, foundMode->Value); |
| } |
| } |
| |
| if (m_whiteBalance.compare(AUTO) == 0) { |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, |
| IMAQdxValueTypeString, AUTO); |
| } else { |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, |
| IMAQdxValueTypeString, MANUAL); |
| if (m_whiteBalanceValuePresent) |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_VALUE, |
| IMAQdxValueTypeU32, m_whiteBalanceValue); |
| } |
| |
| if (m_exposure.compare(AUTO) == 0) { |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, |
| IMAQdxValueTypeString, |
| std::string("AutoAperaturePriority").c_str()); |
| } else { |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_MODE, |
| IMAQdxValueTypeString, MANUAL); |
| if (m_exposureValuePresent) { |
| double minv = 0.0; |
| double maxv = 0.0; |
| SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_EX_VALUE, |
| IMAQdxValueTypeF64, &minv); |
| SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_EX_VALUE, |
| IMAQdxValueTypeF64, &maxv); |
| double val = minv + ((maxv - minv) * ((double)m_exposureValue / 100.0)); |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_EX_VALUE, |
| IMAQdxValueTypeF64, val); |
| } |
| } |
| |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_MODE, IMAQdxValueTypeString, |
| MANUAL); |
| double minv = 0.0; |
| double maxv = 0.0; |
| SAFE_IMAQ_CALL(IMAQdxGetAttributeMinimum, m_id, ATTR_BR_VALUE, |
| IMAQdxValueTypeF64, &minv); |
| SAFE_IMAQ_CALL(IMAQdxGetAttributeMaximum, m_id, ATTR_BR_VALUE, |
| IMAQdxValueTypeF64, &maxv); |
| double val = minv + ((maxv - minv) * ((double)m_brightness / 100.0)); |
| SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_BR_VALUE, IMAQdxValueTypeF64, |
| val); |
| |
| if (wasActive) StartCapture(); |
| } |
| |
| void USBCamera::SetFPS(double fps) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (m_fps != fps) { |
| m_needSettingsUpdate = true; |
| m_fps = fps; |
| } |
| } |
| |
| void USBCamera::SetSize(unsigned int width, unsigned int height) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (m_width != width || m_height != height) { |
| m_needSettingsUpdate = true; |
| m_width = width; |
| m_height = height; |
| } |
| } |
| |
| void USBCamera::SetBrightness(unsigned int brightness) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (m_brightness != brightness) { |
| m_needSettingsUpdate = true; |
| m_brightness = brightness; |
| } |
| } |
| |
| unsigned int USBCamera::GetBrightness() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| return m_brightness; |
| } |
| |
| void USBCamera::SetWhiteBalanceAuto() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_whiteBalance = AUTO; |
| m_whiteBalanceValue = 0; |
| m_whiteBalanceValuePresent = false; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::SetWhiteBalanceHoldCurrent() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_whiteBalance = MANUAL; |
| m_whiteBalanceValue = 0; |
| m_whiteBalanceValuePresent = false; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_whiteBalance = MANUAL; |
| m_whiteBalanceValue = whiteBalance; |
| m_whiteBalanceValuePresent = true; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::SetExposureAuto() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_exposure = AUTO; |
| m_exposureValue = 0; |
| m_exposureValuePresent = false; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::SetExposureHoldCurrent() { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_exposure = MANUAL; |
| m_exposureValue = 0; |
| m_exposureValuePresent = false; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::SetExposureManual(unsigned int level) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| m_exposure = MANUAL; |
| if (level > 100) |
| m_exposureValue = 100; |
| else |
| m_exposureValue = level; |
| m_exposureValuePresent = true; |
| m_needSettingsUpdate = true; |
| } |
| |
| void USBCamera::GetImage(Image* image) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (m_needSettingsUpdate || m_useJpeg) { |
| m_needSettingsUpdate = false; |
| m_useJpeg = false; |
| UpdateSettings(); |
| } |
| // BufNum is not actually used for anything at our level, since we are |
| // waiting until the next image is ready anyway |
| uInt32 bufNum; |
| SAFE_IMAQ_CALL(IMAQdxGrab, m_id, image, 1, &bufNum); |
| } |
| |
| unsigned int USBCamera::GetImageData(void* buffer, unsigned int bufferSize) { |
| std::lock_guard<priority_recursive_mutex> lock(m_mutex); |
| if (m_needSettingsUpdate || !m_useJpeg) { |
| m_needSettingsUpdate = false; |
| m_useJpeg = true; |
| UpdateSettings(); |
| } |
| // BufNum is not actually used for anything at our level |
| uInt32 bufNum; |
| SAFE_IMAQ_CALL(IMAQdxGetImageData, m_id, buffer, bufferSize, |
| IMAQdxBufferNumberModeLast, 0, &bufNum); |
| return GetJpegSize(buffer, bufferSize); |
| } |