diff --git a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java index b2f8cf0f70..4d8ad991ae 100644 --- a/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java +++ b/photon-core/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java @@ -27,6 +27,7 @@ import org.photonvision.common.logging.Logger; import org.photonvision.vision.calibration.CameraCalibrationCoefficients; import org.photonvision.vision.camera.CameraType; +import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.pipeline.CVPipelineSettings; import org.photonvision.vision.pipeline.DriverModePipelineSettings; import org.photonvision.vision.processes.PipelineManager; @@ -46,6 +47,8 @@ public class CameraConfiguration { /** Can be either path (ex /dev/videoX) or index (ex 1). */ public String path = ""; + public QuirkyCamera cameraQuirks; + @JsonIgnore public String[] otherPaths = {}; public CameraType cameraType = CameraType.UsbCamera; @@ -93,6 +96,7 @@ public CameraConfiguration( @JsonProperty("FOV") double FOV, @JsonProperty("path") String path, @JsonProperty("cameraType") CameraType cameraType, + @JsonProperty("cameraQuirks") QuirkyCamera cameraQuirks, @JsonProperty("calibration") List calibrations, @JsonProperty("currentPipelineIndex") int currentPipelineIndex) { this.baseName = baseName; @@ -101,6 +105,7 @@ public CameraConfiguration( this.FOV = FOV; this.path = path; this.cameraType = cameraType; + this.cameraQuirks = cameraQuirks; this.calibrations = calibrations != null ? calibrations : new ArrayList<>(); this.currentPipelineIndex = currentPipelineIndex; @@ -165,6 +170,8 @@ public String toString() { + Arrays.toString(otherPaths) + ", cameraType=" + cameraType + + ", cameraQuirks=" + + cameraQuirks + ", FOV=" + FOV + ", calibrations=" diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java b/photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java index 928aa6f847..5c2e4a6136 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/CameraQuirk.java @@ -32,4 +32,9 @@ public enum CameraQuirk { AdjustableFocus, /** Changing FPS repeatedly with small delay does not work correctly */ StickyFPS, + /** + * Camera is an arducam ov9281 which has a funky exposure issue where it is defined in v4l as + * 1-5000 instead of 1-75 + */ + ArduOV9281, } diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java index 05737ed293..d3c96d1b31 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/QuirkyCamera.java @@ -17,6 +17,8 @@ package org.photonvision.vision.camera; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonProperty; import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; @@ -47,7 +49,12 @@ public class QuirkyCamera { -1, -1, "mmal service 16.1", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy) new QuirkyCamera(-1, -1, "unicam", CameraQuirk.PiCam), // PiCam (via V4L2, not zerocopy) new QuirkyCamera(0x85B, 0x46D, CameraQuirk.AdjustableFocus), // Logitech C925-e - new QuirkyCamera(0x6366, 0x0c45, CameraQuirk.StickyFPS) // Arducam OV2311 + new QuirkyCamera( + 0x6366, 0x0c45, "", "Arducam Generic"), + new QuirkyCamera( + 0x6366, 0x0c45, "OV2311", "OV2311", CameraQuirk.StickyFPS), // Arducam OV2311 + new QuirkyCamera( + 0x6366, 0x0c45, "OV9281", "OV9281", CameraQuirk.ArduOV9281) // Arducam OV9281 ); public static final QuirkyCamera DefaultCamera = new QuirkyCamera(0, 0, ""); @@ -63,6 +70,7 @@ public class QuirkyCamera { public final String baseName; public final int usbVid; public final int usbPid; + public final String displayName; public final HashMap quirks; /** @@ -85,9 +93,23 @@ private QuirkyCamera(int usbVid, int usbPid, CameraQuirk... quirks) { * @param quirks Camera quirks */ private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk... quirks) { + this(usbVid, usbPid, baseName, "", quirks); + } + + /** + * Creates a QuirkyCamera that matches by USB VID/PID and name + * + * @param usbVid USB VID of camera + * @param usbPid USB PID of camera + * @param baseName CSCore name of camera + * @param quirks Camera quirks + */ + private QuirkyCamera( + int usbVid, int usbPid, String baseName, String displayName, CameraQuirk... quirks) { this.usbVid = usbVid; this.usbPid = usbPid; this.baseName = baseName; + this.displayName = displayName; this.quirks = new HashMap<>(); for (var q : quirks) { @@ -98,6 +120,20 @@ private QuirkyCamera(int usbVid, int usbPid, String baseName, CameraQuirk... qui } } + @JsonCreator + public QuirkyCamera( + @JsonProperty("baseName") String baseName, + @JsonProperty("usbVid") int usbVid, + @JsonProperty("usbPid") int usbPid, + @JsonProperty("usbPid") String displayName, + @JsonProperty("quirks") HashMap quirks) { + this.baseName = baseName; + this.usbPid = usbPid; + this.usbVid = usbVid; + this.quirks = quirks; + this.displayName = displayName; + } + public boolean hasQuirk(CameraQuirk quirk) { return quirks.get(quirk); } @@ -144,6 +180,23 @@ public boolean equals(Object o) { && Objects.equals(quirks, that.quirks); } + @Override + public String toString() { + String ret = + "QuirkyCamera [baseName=" + + baseName + + ", displayName=" + + displayName + + ", usbVid=" + + usbVid + + ", usbPid=" + + usbPid + + ", quirks=" + + quirks.toString() + + "]"; + return ret; + } + @Override public int hashCode() { return Objects.hash(usbVid, usbPid, baseName, quirks); diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java index 69f17f52fd..977343537b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java +++ b/photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java @@ -44,8 +44,6 @@ public class USBCameraSource extends VisionSource { private FrameProvider usbFrameProvider; private final CvSink cvSink; - private QuirkyCamera cameraQuirks; - public USBCameraSource(CameraConfiguration config) { super(config); @@ -53,17 +51,21 @@ public USBCameraSource(CameraConfiguration config) { camera = new UsbCamera(config.nickname, config.path); cvSink = CameraServer.getVideo(this.camera); - cameraQuirks = - QuirkyCamera.getQuirkyCamera( - camera.getInfo().productId, camera.getInfo().vendorId, config.baseName); + if (getCameraConfiguration().cameraQuirks == null) + getCameraConfiguration().cameraQuirks = + QuirkyCamera.getQuirkyCamera( + camera.getInfo().vendorId, camera.getInfo().productId, config.baseName); - if (cameraQuirks.hasQuirks()) { - logger.info("Quirky camera detected: " + cameraQuirks.baseName); + if (getCameraConfiguration().cameraQuirks.hasQuirks()) { + logger.info("Quirky camera detected: " + getCameraConfiguration().cameraQuirks.baseName); } - if (cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.CompletelyBroken)) { // set some defaults, as these should never be used. - logger.info("Camera " + cameraQuirks.baseName + " is not supported for PhotonVision"); + logger.info( + "Camera " + + getCameraConfiguration().cameraQuirks.baseName + + " is not supported for PhotonVision"); usbCameraSettables = null; usbFrameProvider = null; } else { @@ -87,7 +89,9 @@ public USBCameraSource(CameraConfiguration config) { public USBCameraSource(CameraConfiguration config, int pid, int vid, boolean unitTest) { this(config); - cameraQuirks = QuirkyCamera.getQuirkyCamera(pid, vid, config.baseName); + if (getCameraConfiguration().cameraQuirks == null) + getCameraConfiguration().cameraQuirks = + QuirkyCamera.getQuirkyCamera(pid, vid, config.baseName); if (unitTest) usbFrameProvider = @@ -98,7 +102,7 @@ public USBCameraSource(CameraConfiguration config, int pid, int vid, boolean uni } void disableAutoFocus() { - if (cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.AdjustableFocus)) { try { camera.getProperty("focus_auto").set(0); camera.getProperty("focus_absolute").set(0); // Focus into infinity @@ -109,7 +113,7 @@ void disableAutoFocus() { } public QuirkyCamera getCameraQuirks() { - return this.cameraQuirks; + return getCameraConfiguration().cameraQuirks; } @Override @@ -130,14 +134,14 @@ public class USBCameraSettables extends VisionSourceSettables { protected USBCameraSettables(CameraConfiguration configuration) { super(configuration); getAllVideoModes(); - if (!cameraQuirks.hasQuirk(CameraQuirk.StickyFPS)) + if (!configuration.cameraQuirks.hasQuirk(CameraQuirk.StickyFPS)) if (!videoModes.isEmpty()) setVideoMode(videoModes.get(0)); // fixes double FPS set } public void setAutoExposure(boolean cameraAutoExposure) { logger.debug("Setting auto exposure to " + cameraAutoExposure); - if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { // Case, we know this is a picam. Go through v4l2-ctl interface directly // Common settings @@ -169,7 +173,7 @@ public void setAutoExposure(boolean cameraAutoExposure) { } else { // Case - this is some other USB cam. Default to wpilib's implementation - var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain); + var canSetWhiteBalance = !getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.Gain); if (!cameraAutoExposure) { // Pick a bunch of reasonable setting defaults for vision processing retroreflective @@ -236,7 +240,7 @@ public void setExposure(double exposure) { if (exposure >= 0.0) { try { int scaledExposure = 1; - if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { scaledExposure = Math.round(timeToPiCamRawExposure(pctToExposureTimeUs(exposure))); logger.debug("Setting camera raw exposure to " + scaledExposure); camera.getProperty("raw_exposure_time_absolute").set(scaledExposure); @@ -249,9 +253,17 @@ public void setExposure(double exposure) { camera.getProperty("auto_exposure").set(1); // Seems like the name changed at some point in v4l? set it ouyrselves too - var prop = camera.getProperty("exposure_time_absolute"); - var exposure_manual_val = - MathUtils.map(Math.round(exposure), 0, 100, prop.getMin(), prop.getMax()); + var prop = camera.getProperty("raw_exposure_time_absolute"); + + var propMin = prop.getMin(); + var propMax = prop.getMax(); + + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.ArduOV9281)) { + propMin = 1; + propMax = 75; + } + + var exposure_manual_val = MathUtils.map(Math.round(exposure), 0, 100, propMin, propMax); prop.set((int) exposure_manual_val); } else { scaledExposure = (int) Math.round(exposure); @@ -279,7 +291,7 @@ public void setBrightness(int brightness) { @Override public void setGain(int gain) { try { - if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.Gain)) { camera.getProperty("gain_automatic").set(0); camera.getProperty("gain").set(gain); } @@ -313,7 +325,7 @@ public HashMap getAllVideoModes() { List videoModesList = new ArrayList<>(); try { VideoMode[] modes; - if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { modes = new VideoMode[] { new VideoMode(VideoMode.PixelFormat.kBGR, 320, 240, 90), @@ -341,13 +353,13 @@ public HashMap getAllVideoModes() { } // On picam, filter non-bgr modes for performance - if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.PiCam)) { if (videoMode.pixelFormat != VideoMode.PixelFormat.kBGR) { continue; } } - if (cameraQuirks.hasQuirk(CameraQuirk.FPSCap100)) { + if (getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.FPSCap100)) { if (videoMode.fps > 100) { continue; } @@ -405,7 +417,7 @@ public HashMap getAllVideoModes() { @Override public boolean isVendorCamera() { return ConfigManager.getInstance().getConfig().getHardwareConfig().hasPresetFOV() - && cameraQuirks.hasQuirk(CameraQuirk.PiCam); + && getCameraConfiguration().cameraQuirks.hasQuirk(CameraQuirk.PiCam); } @Override @@ -426,15 +438,22 @@ public boolean equals(Object obj) { if (cvSink == null) { if (other.cvSink != null) return false; } else if (!cvSink.equals(other.cvSink)) return false; - if (cameraQuirks == null) { - if (other.cameraQuirks != null) return false; - } else if (!cameraQuirks.equals(other.cameraQuirks)) return false; + if (getCameraConfiguration().cameraQuirks == null) { + if (other.getCameraConfiguration().cameraQuirks != null) return false; + } else if (!getCameraConfiguration() + .cameraQuirks + .equals(other.getCameraConfiguration().cameraQuirks)) return false; return true; } @Override public int hashCode() { return Objects.hash( - camera, usbCameraSettables, usbFrameProvider, cameraConfiguration, cvSink, cameraQuirks); + camera, + usbCameraSettables, + usbFrameProvider, + cameraConfiguration, + cvSink, + getCameraConfiguration().cameraQuirks); } } diff --git a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java index 18d4ae5b6a..286bde724d 100644 --- a/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java +++ b/photon-core/src/test/java/org/photonvision/common/configuration/SQLConfigTest.java @@ -25,6 +25,7 @@ import org.junit.jupiter.api.Test; import org.photonvision.common.util.TestUtils; import org.photonvision.vision.camera.CameraType; +import org.photonvision.vision.camera.QuirkyCamera; import org.photonvision.vision.pipeline.AprilTagPipelineSettings; import org.photonvision.vision.pipeline.ColoredShapePipelineSettings; import org.photonvision.vision.pipeline.ReflectivePipelineSettings; @@ -49,6 +50,7 @@ public void testLoad() { 69, "a/path/idk", CameraType.UsbCamera, + QuirkyCamera.getQuirkyCamera(-1, -1), List.of(), 0); testcamcfg.pipelineSettings =