add DigitalSource::Get so DigitalSource is actually usable
This is our wpilib commit 1e9a531ce33128440fc9030a5a852bc8bbf65b91 and
still uses FPGA image v23.
This also includes adding 2 files I missed in previous wpilib changes.
Change-Id: Ief3ce091c600c290129c9ac8f66cdb0766d23df1
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/AnalogTriggerOutput.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/AnalogTriggerOutput.h
index e4d8cd5..8ab701e 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/AnalogTriggerOutput.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/AnalogTriggerOutput.h
@@ -42,7 +42,7 @@
public:
virtual ~AnalogTriggerOutput();
- bool Get();
+ virtual bool Get();
// DigitalSource interface
virtual uint32_t GetChannelForRouting();
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalInput.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalInput.h
index 2b548d3..928d71e 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalInput.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalInput.h
@@ -25,7 +25,7 @@
public:
explicit DigitalInput(uint32_t channel);
virtual ~DigitalInput();
- bool Get();
+ virtual bool Get();
uint32_t GetChannel();
// Digital Source Interface
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalOutput.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalOutput.h
index c04ef05..dc6012b 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalOutput.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalOutput.h
@@ -18,6 +18,7 @@
explicit DigitalOutput(uint32_t channel);
virtual ~DigitalOutput();
void Set(uint32_t value);
+ virtual bool Get();
uint32_t GetChannel();
void Pulse(float length);
bool IsPulsing();
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalSource.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalSource.h
index 491fa7c..066bc63 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalSource.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/DigitalSource.h
@@ -18,6 +18,7 @@
{
public:
virtual ~DigitalSource();
+ virtual bool Get() = 0;
virtual uint32_t GetChannelForRouting() = 0;
virtual uint32_t GetModuleForRouting() = 0;
virtual bool GetAnalogTriggerForRouting() = 0;
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/USBCamera.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/USBCamera.h
new file mode 100644
index 0000000..ac8bfe4
--- /dev/null
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/USBCamera.h
@@ -0,0 +1,111 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+#pragma once
+
+#include "ErrorBase.h"
+#include "nivision.h"
+#include "NIIMAQdx.h"
+
+#include <mutex>
+#include <string>
+
+typedef enum whiteBalance_enum {
+ kFixedIndoor = 3000,
+ kFixedOutdoor1 = 4000,
+ kFixedOutdoor2 = 5000,
+ kFixedFluorescent1 = 5100,
+ kFixedFlourescent2 = 5200
+} whiteBalance;
+
+class USBCamera : public ErrorBase {
+ private:
+ static constexpr char const *ATTR_WB_MODE = "CameraAttributes::WhiteBalance::Mode";
+ static constexpr char const *ATTR_WB_VALUE = "CameraAttributes::WhiteBalance::Value";
+ static constexpr char const *ATTR_EX_MODE = "CameraAttributes::Exposure::Mode";
+ static constexpr char const *ATTR_EX_VALUE = "CameraAttributes::Exposure::Value";
+ static constexpr char const *ATTR_BR_MODE = "CameraAttributes::Brightness::Mode";
+ static constexpr char const *ATTR_BR_VALUE = "CameraAttributes::Brightness::Value";
+
+ protected:
+ IMAQdxSession m_id;
+ std::string m_name;
+ bool m_useJpeg;
+ bool m_active;
+ bool m_open;
+
+ std::recursive_mutex m_mutex;
+
+ unsigned int m_width;
+ unsigned int m_height;
+ double m_fps;
+ std::string m_whiteBalance;
+ unsigned int m_whiteBalanceValue;
+ bool m_whiteBalanceValuePresent;
+ std::string m_exposure;
+ unsigned int m_exposureValue;
+ bool m_exposureValuePresent;
+ unsigned int m_brightness;
+ bool m_needSettingsUpdate;
+
+ unsigned int GetJpegSize(void* buffer, unsigned int buffSize);
+
+ public:
+ static constexpr char const *kDefaultCameraName = "cam0";
+
+ USBCamera(std::string name, bool useJpeg);
+
+ void OpenCamera();
+ void CloseCamera();
+ void StartCapture();
+ void StopCapture();
+ void SetFPS(double fps);
+ void SetSize(unsigned int width, unsigned int height);
+
+ void UpdateSettings();
+ /**
+ * Set the brightness, as a percentage (0-100).
+ */
+ void SetBrightness(unsigned int brightness);
+
+ /**
+ * Get the brightness, as a percentage (0-100).
+ */
+ unsigned int GetBrightness();
+
+ /**
+ * Set the white balance to auto
+ */
+ void SetWhiteBalanceAuto();
+
+ /**
+ * Set the white balance to hold current
+ */
+ void SetWhiteBalanceHoldCurrent();
+
+ /**
+ * Set the white balance to manual, with specified color temperature
+ */
+ void SetWhiteBalanceManual(unsigned int wbValue);
+
+ /**
+ * Set the exposure to auto exposure
+ */
+ void SetExposureAuto();
+
+ /**
+ * Set the exposure to hold current
+ */
+ void SetExposureHoldCurrent();
+
+ /**
+ * Set the exposure to manual, with a given percentage (0-100)
+ */
+ void SetExposureManual(unsigned int expValue);
+
+ void GetImage(Image* image);
+ unsigned int GetImageData(void* buffer, unsigned int bufferSize);
+};
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalOutput.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalOutput.cpp
index a3fc2d1..aeeab8a 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalOutput.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DigitalOutput.cpp
@@ -74,6 +74,18 @@
}
/**
+ * Get the value from a digital output channel.
+ * Retrieve the value of a single digital output channel from the FPGA.
+ */
+bool DigitalOutput::Get()
+{
+ int32_t status = 0;
+ bool value = getDIO(m_digital_ports[m_channel], &status);
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+ return value;
+}
+
+/**
* @return The GPIO channel number that this object represents.
*/
uint32_t DigitalOutput::GetChannel()
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/USBCamera.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/USBCamera.cpp
new file mode 100644
index 0000000..8d36be1
--- /dev/null
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/USBCamera.cpp
@@ -0,0 +1,328 @@
+#include "USBCamera.h"
+
+#include "Utility.h"
+
+#include <regex>
+#include <chrono>
+#include <thread>
+#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); \
+ }
+
+// Constants for the manual and auto types
+static const std::string AUTO = "Auto";
+static const std::string MANUAL = "Manual";
+
+/**
+ * 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_id(0),
+ m_name(name),
+ m_useJpeg(useJpeg),
+ m_active(false),
+ m_open(false),
+ m_mutex(),
+ m_width(320),
+ m_height(240),
+ m_fps(30),
+ m_whiteBalance(AUTO),
+ m_whiteBalanceValue(0),
+ m_whiteBalanceValuePresent(false),
+ m_exposure(MANUAL),
+ m_exposureValue(50),
+ m_exposureValuePresent(false),
+ m_brightness(80),
+ m_needSettingsUpdate(true) {
+}
+
+void USBCamera::OpenCamera() {
+ std::unique_lock<std::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::unique_lock<std::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::unique_lock<std::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::unique_lock<std::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::unique_lock<std::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, NULL, &count, ¤tMode);
+ IMAQdxVideoMode modes[count];
+ SAFE_IMAQ_CALL(IMAQdxEnumerateVideoModes, m_id, modes, &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.c_str());
+ } else {
+ SAFE_IMAQ_CALL(IMAQdxSetAttribute, m_id, ATTR_WB_MODE, IMAQdxValueTypeString, MANUAL.c_str());
+ 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.c_str());
+ 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.c_str());
+ 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::unique_lock<std::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::unique_lock<std::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::unique_lock<std::recursive_mutex> lock(m_mutex);
+ if (m_brightness != brightness) {
+ m_needSettingsUpdate = true;
+ m_brightness = brightness;
+ }
+}
+
+unsigned int USBCamera::GetBrightness() {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ return m_brightness;
+}
+
+void USBCamera::SetWhiteBalanceAuto() {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ m_whiteBalance = AUTO;
+ m_whiteBalanceValue = 0;
+ m_whiteBalanceValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceHoldCurrent() {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ m_whiteBalance = MANUAL;
+ m_whiteBalanceValue = 0;
+ m_whiteBalanceValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ m_whiteBalance = MANUAL;
+ m_whiteBalanceValue = whiteBalance;
+ m_whiteBalanceValuePresent = true;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureAuto() {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ m_exposure = AUTO;
+ m_exposureValue = 0;
+ m_exposureValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureHoldCurrent() {
+ std::unique_lock<std::recursive_mutex> lock(m_mutex);
+ m_exposure = MANUAL;
+ m_exposureValue = 0;
+ m_exposureValuePresent = false;
+ m_needSettingsUpdate = true;
+}
+
+void USBCamera::SetExposureManual(unsigned int level) {
+ std::unique_lock<std::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::unique_lock<std::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::unique_lock<std::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);
+}