[Wip] Add auto exposure switch (#488)

* Add auto exposure switch

* Run wpiformat

* Update ZeroCopyPicamSource.java

photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java

@@ -91,15 +91,14 @@ public class FileVisionSource extends VisionSource {
         @Override
         public void setExposure(double exposure) {}
 
+        public void setAutoExposure(boolean cameraAutoExposure) {}
+
         @Override
         public void setBrightness(int brightness) {}
 
         @Override
         public void setGain(int gain) {}
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {}
-
         @Override
         public VideoMode getCurrentVideoMode() {
             return videoMode;

photon-core/src/main/java/org/photonvision/vision/camera/USBCameraSource.java

@@ -61,8 +61,9 @@ public class USBCameraSource extends VisionSource {
             usbFrameProvider = null;
         } else {
             // Normal init
-            setLowExposureOptimizationImpl(false);
+            // auto exposure/brightness/gain will be set by the visionmodule later
             disableAutoFocus();
+
             usbCameraSettables = new USBCameraSettables(config);
             usbFrameProvider = new USBFrameProvider(cvSink, usbCameraSettables);
         }
@@ -79,58 +80,6 @@ public class USBCameraSource extends VisionSource {
         }
     }
 
-    void setLowExposureOptimizationImpl(boolean lowExposureMode) {
-        if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
-            // Case, we know this is a picam. Go through v4l2-ctl interface directly
-
-            // Common settings
-            camera
-                    .getProperty("image_stabilization")
-                    .set(0); // No image stabilization, as this will throw off odometry
-            camera.getProperty("power_line_frequency").set(2); // Assume 60Hz USA
-            camera.getProperty("scene_mode").set(0); // no presets
-            camera.getProperty("exposure_metering_mode").set(0);
-            camera.getProperty("exposure_dynamic_framerate").set(0);
-
-            if (lowExposureMode) {
-                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
-                camera.getProperty("auto_exposure_bias").set(0);
-                camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement
-                camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement
-                camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled
-                camera.getProperty("auto_exposure").set(1); // auto exposure disabled
-            } else {
-                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
-                // nice-for-humans
-                camera.getProperty("auto_exposure_bias").set(12);
-                camera.getProperty("iso_sensitivity_auto").set(1);
-                camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default
-                camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled
-                camera.getProperty("auto_exposure").set(0); // auto exposure enabled
-            }
-
-        } else {
-            // Case - this is some other USB cam. Default to wpilib's implementation
-
-            var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain);
-
-            if (lowExposureMode) {
-                // Pick a bunch of reasonable setting defaults for vision processing retroreflective
-                if (canSetWhiteBalance) {
-                    camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset
-                }
-                this.getSettables().setExposure(50); // auto exposure disabled, put a sane default
-            } else {
-                // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
-                // nice-for-humans
-                if (canSetWhiteBalance) {
-                    camera.setWhiteBalanceAuto(); // Auto white-balance enabled
-                }
-                camera.setExposureAuto(); // auto exposure enabled
-            }
-        }
-    }
-
     @Override
     public FrameProvider getFrameProvider() {
         return usbFrameProvider;
@@ -148,6 +97,59 @@ public class USBCameraSource extends VisionSource {
             setVideoMode(videoModes.get(0));
         }
 
+        public void setAutoExposure(boolean cameraAutoExposure) {
+            logger.debug("Setting auto exposure to " + cameraAutoExposure);
+
+            if (cameraQuirks.hasQuirk(CameraQuirk.PiCam)) {
+                // Case, we know this is a picam. Go through v4l2-ctl interface directly
+
+                // Common settings
+                camera
+                        .getProperty("image_stabilization")
+                        .set(0); // No image stabilization, as this will throw off odometry
+                camera.getProperty("power_line_frequency").set(2); // Assume 60Hz USA
+                camera.getProperty("scene_mode").set(0); // no presets
+                camera.getProperty("exposure_metering_mode").set(0);
+                camera.getProperty("exposure_dynamic_framerate").set(0);
+
+                if (!cameraAutoExposure) {
+                    // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                    camera.getProperty("auto_exposure_bias").set(0);
+                    camera.getProperty("iso_sensitivity_auto").set(0); // Disable auto ISO adjustement
+                    camera.getProperty("iso_sensitivity").set(0); // Manual ISO adjustement
+                    camera.getProperty("white_balance_auto_preset").set(2); // Auto white-balance disabled
+                    camera.getProperty("auto_exposure").set(1); // auto exposure disabled
+                } else {
+                    // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                    // nice-for-humans
+                    camera.getProperty("auto_exposure_bias").set(12);
+                    camera.getProperty("iso_sensitivity_auto").set(1);
+                    camera.getProperty("iso_sensitivity").set(1); // Manual ISO adjustement by default
+                    camera.getProperty("white_balance_auto_preset").set(1); // Auto white-balance enabled
+                    camera.getProperty("auto_exposure").set(0); // auto exposure enabled
+                }
+
+            } else {
+                // Case - this is some other USB cam. Default to wpilib's implementation
+
+                var canSetWhiteBalance = !cameraQuirks.hasQuirk(CameraQuirk.Gain);
+
+                if (!cameraAutoExposure) {
+                    // Pick a bunch of reasonable setting defaults for vision processing retroreflective
+                    if (canSetWhiteBalance) {
+                        camera.setWhiteBalanceManual(4000); // Auto white-balance disabled, 4000K preset
+                    }
+                } else {
+                    // Pick a bunch of reasonable setting defaults for driver, fiducials, or otherwise
+                    // nice-for-humans
+                    if (canSetWhiteBalance) {
+                        camera.setWhiteBalanceAuto(); // Auto white-balance enabled
+                    }
+                    camera.setExposureAuto(); // auto exposure enabled
+                }
+            }
+        }
+
         private int timeToPiCamRawExposure(double time_us) {
             int retVal =
                     (int)
@@ -166,11 +168,6 @@ public class USBCameraSource extends VisionSource {
                     + (pct_in / 100.0) * ((1e6 / (double) camera.getVideoMode().fps) - PADDING_HIGH_US);
         }
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {
-            setLowExposureOptimizationImpl(mode);
-        }
-
         @Override
         public void setExposure(double exposure) {
             if (exposure >= 0.0) {

photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java

@@ -45,13 +45,6 @@ public class ZeroCopyPicamSource extends VisionSource {
 
         settables = new PicamSettables(configuration);
         frameProvider = new AcceleratedPicamFrameProvider(settables);
-
-        setLowExposureOptimizationImpl(false);
-    }
-
-    static void setLowExposureOptimizationImpl(boolean mode) {
-        // TODO - ZeroCopy does not... yet? ... have the configuration params necessary to make this
-        // work well.
     }
 
     @Override
@@ -94,6 +87,7 @@ public class ZeroCopyPicamSource extends VisionSource {
         private FPSRatedVideoMode currentVideoMode;
         private double lastExposure = 50;
         private int lastBrightness = 50;
+        private boolean lastExposureMode;
         private int lastGain = 50;
         private Pair<Integer, Integer> lastAwbGains = new Pair(18, 18);
 
@@ -150,9 +144,15 @@ public class ZeroCopyPicamSource extends VisionSource {
             return getCurrentVideoMode().fovMultiplier * getConfiguration().FOV;
         }
 
+        @Override
+        public void setAutoExposure(boolean cameraAutoExposure) {
+            lastExposureMode = cameraAutoExposure;
+            // TODO (Matt) -- call PicamJNI's auto exposure function, when that exists
+        }
+
         @Override
         public void setExposure(double exposure) {
-            // Todo - for now, handle auto exposure by using 100% exposure
+            // Todo (Chris) - for now, handle auto exposure by using 100% exposure
             if (exposure < 0.0) {
                 exposure = 100.0;
             }
@@ -162,11 +162,6 @@ public class ZeroCopyPicamSource extends VisionSource {
             if (failure) logger.warn("Couldn't set Pi Camera exposure");
         }
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {
-            setLowExposureOptimizationImpl(mode);
-        }
-
         @Override
         public void setBrightness(int brightness) {
             lastBrightness = brightness;
@@ -218,6 +213,7 @@ public class ZeroCopyPicamSource extends VisionSource {
             // We don't store last settings on the native side, and when you change video mode these get
             // reset on MMAL's end
             setExposure(lastExposure);
+            setAutoExposure(lastExposureMode);
             setBrightness(lastBrightness);
             setGain(lastGain);
             setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());

photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java

@@ -40,6 +40,7 @@ public class AprilTagPipelineSettings extends AdvancedPipelineSettings {
         outputShowMultipleTargets = true;
         targetModel = TargetModel.k200mmAprilTag;
         cameraExposure = -1;
+        cameraAutoExposure = true;
         ledMode = false;
     }
 

photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java

@@ -40,7 +40,8 @@ public class CVPipelineSettings implements Cloneable {
     public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
     public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
     public String pipelineNickname = "New Pipeline";
-    // Only used if the pipeline type does not enable auto-exposure
+    public boolean cameraAutoExposure = false;
+    // manual exposure only used if cameraAutoExposure if false
     public double cameraExposure = 100;
     public int cameraBrightness = 50;
     // Currently only used by a few cameras (notably the zero-copy Pi Camera driver) with the Gain

photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java

@@ -44,7 +44,6 @@ import org.photonvision.vision.frame.consumer.FileSaveFrameConsumer;
 import org.photonvision.vision.frame.consumer.MJPGFrameConsumer;
 import org.photonvision.vision.pipeline.AdvancedPipelineSettings;
 import org.photonvision.vision.pipeline.OutputStreamPipeline;
-import org.photonvision.vision.pipeline.PipelineType;
 import org.photonvision.vision.pipeline.ReflectivePipelineSettings;
 import org.photonvision.vision.pipeline.UICalibrationData;
 import org.photonvision.vision.pipeline.result.CVPipelineResult;
@@ -361,7 +360,7 @@ public class VisionModule {
             settings.cameraBlueGain = -1;
         }
 
-        settings.cameraExposure = -1;
+        settings.cameraAutoExposure = true;
 
         setPipeline(PipelineManager.CAL_3D_INDEX);
     }
@@ -400,22 +399,14 @@ public class VisionModule {
         visionSource.getSettables().setBrightness(pipelineSettings.cameraBrightness);
         visionSource.getSettables().setGain(pipelineSettings.cameraGain);
 
-        // set to true to change camera gain/exposure settings for low exposure
-        // false will keep the camera running in a more "nice-for-humans" mode
-        // Each camera type is allowed to decide what settings that exactly means
-        boolean lowExposureOptimization =
-                (pipelineSettings.pipelineType == PipelineType.ColoredShape
-                        || pipelineSettings.pipelineType == PipelineType.Reflective);
-        visionSource.getSettables().setLowExposureOptimization(lowExposureOptimization);
-
-        if (lowExposureOptimization) {
-            if (pipelineSettings.cameraExposure == -1)
+        // If manual exposure, force exposure slider to be valid
+        if (!pipelineSettings.cameraAutoExposure) {
+            if (pipelineSettings.cameraExposure < 0)
                 pipelineSettings.cameraExposure = 10; // reasonable default
-        } else {
-            // in human-friendly mode, exposure is automatic
-            pipelineSettings.cameraExposure = -1;
         }
+
         visionSource.getSettables().setExposure(pipelineSettings.cameraExposure);
+        visionSource.getSettables().setAutoExposure(pipelineSettings.cameraAutoExposure);
 
         if (cameraQuirks.hasQuirk(CameraQuirk.Gain)) {
             // If the gain is disabled for some reason, re-enable it

photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java

@@ -44,6 +44,8 @@ public abstract class VisionSourceSettables {
 
     public abstract void setExposure(double exposure);
 
+    public abstract void setAutoExposure(boolean cameraAutoExposure);
+
     public abstract void setBrightness(int brightness);
 
     public abstract void setGain(int gain);
@@ -64,8 +66,6 @@ public abstract class VisionSourceSettables {
         calculateFrameStaticProps();
     }
 
-    public abstract void setLowExposureOptimization(boolean mode);
-
     protected abstract void setVideoModeInternal(VideoMode videoMode);
 
     @SuppressWarnings("unused")

photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java

@@ -78,9 +78,6 @@ public class VisionModuleManagerTest {
         @Override
         public void setGain(int gain) {}
 
-        @Override
-        public void setLowExposureOptimization(boolean mode) {}
-
         @Override
         public VideoMode getCurrentVideoMode() {
             return new VideoMode(0, 320, 240, 254);
@@ -97,6 +94,9 @@ public class VisionModuleManagerTest {
             ret.put(0, getCurrentVideoMode());
             return ret;
         }
+
+        @Override
+        public void setAutoExposure(boolean cameraAutoExposure) {}
     }
 
     private static class TestDataConsumer implements CVPipelineResultConsumer {