From 87e7c3ca746402f701d2d20a2497061a6f97c360 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Sun, 9 Oct 2022 22:41:40 -0400
Subject: [PATCH] [Wip] Add auto exposure switch (#488)

* Add auto exposure switch

* Run wpiformat

* Update ZeroCopyPicamSource.java
---
 photon-client/src/store/index.js              |   1 +
 .../src/views/PipelineViews/InputTab.vue      |  33 ++++-
 .../vision/camera/FileVisionSource.java       |   5 +-
 .../vision/camera/USBCameraSource.java        | 113 +++++++++---------
 .../vision/camera/ZeroCopyPicamSource.java    |  22 ++--
 .../pipeline/AprilTagPipelineSettings.java    |   1 +
 .../vision/pipeline/CVPipelineSettings.java   |   3 +-
 .../vision/processes/VisionModule.java        |  21 +---
 .../processes/VisionSourceSettables.java      |   4 +-
 .../processes/VisionModuleManagerTest.java    |   6 +-
 10 files changed, 111 insertions(+), 98 deletions(-)

diff --git a/photon-client/src/store/index.js b/photon-client/src/store/index.js
index fcf70bbf5..086b75260 100644
--- a/photon-client/src/store/index.js
+++ b/photon-client/src/store/index.js
@@ -57,6 +57,7 @@ export default new Vuex.Store({
                     // Settings that apply to all pipeline types
                     cameraExposure: 1,
                     cameraBrightness: 2,
+                    cameraAutoExposure: false,
                     cameraRedGain: 3,
                     cameraBlueGain: 4,
                     inputImageRotationMode: 0,
diff --git a/photon-client/src/views/PipelineViews/InputTab.vue b/photon-client/src/views/PipelineViews/InputTab.vue
index ca7cd5c67..f1a222c68 100644
--- a/photon-client/src/views/PipelineViews/InputTab.vue
+++ b/photon-client/src/views/PipelineViews/InputTab.vue
@@ -1,7 +1,7 @@
 <template>
   <div>
     <CVslider
-      v-if="cameraExposure !== -1"
+      :disabled="cameraAutoExposure"
       v-model="cameraExposure"
       name="Exposure"
       min="0"
@@ -22,10 +22,27 @@
       @input="handlePipelineData('cameraBrightness')"
       @rollback="e => rollback('cameraBrightness', e)"
     />
+    <CVswitch
+      v-model="cameraAutoExposure"
+      class="pt-2"
+      name="Auto exposure"
+      @input="handlePipelineData('cameraAutoExposure')"
+    />
+    <CVslider
+      v-if="cameraGain >= 0"
+      v-model="cameraGain"
+      name="Camera gain"
+      min="0"
+      max="100"
+      tooltip="Controls camera gain, similar to brightness"
+      :slider-cols="largeBox"
+      @input="handlePipelineData('cameraRedGain')"
+      @rollback="e => rollback('cameraRedGain', e)"
+    />
     <CVslider
       v-if="cameraRedGain !== -1"
       v-model="cameraRedGain"
-      name="Red AWB Gain"
+      name="Red Balance"
       min="0"
       max="100"
       tooltip="Controls red automatic white balance gain, which affects how the camera captures colors in different conditions"
@@ -36,7 +53,7 @@
     <CVslider
       v-if="cameraBlueGain !== -1"
       v-model="cameraBlueGain"
-      name="Blue AWB Gain"
+      name="Blue Balance"
       min="0"
       max="100"
       tooltip="Controls blue automatic white balance gain, which affects how the camera captures colors in different conditions"
@@ -76,6 +93,7 @@
 <script>
     import CVslider from '../../components/common/cv-slider'
     import CVselect from '../../components/common/cv-select'
+    import CVswitch from '../../components/common/cv-switch'
 
     const unfilteredStreamDivisors = [1, 2, 4, 6];
 
@@ -84,6 +102,7 @@
         components: {
             CVslider,
             CVselect,
+            CVswitch,
         },
         // eslint-disable-next-line vue/require-prop-types
         props: ['value'],
@@ -109,6 +128,14 @@
                     this.$store.commit("mutatePipeline", {"cameraExposure": parseFloat(val)});
                 }
             },
+            cameraAutoExposure: {
+                get() {
+                    return this.$store.getters.currentPipelineSettings.cameraAutoExposure;
+                },
+                set(val) {
+                    this.$store.commit("mutatePipeline", {"cameraAutoExposure": val});
+                }
+            },
             cameraBrightness: {
                 get() {
                     return parseInt(this.$store.getters.currentPipelineSettings.cameraBrightness)
diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
index c7ebb9e7b..e028dfe36 100644
--- a/photon-core/src/main/java/org/photonvision/vision/camera/FileVisionSource.java
+++ b/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;
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 88ff29eb4..f74eeeea2 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
@@ -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) {
diff --git a/photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java b/photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java
index a12cb2420..c7e980696 100644
--- a/photon-core/src/main/java/org/photonvision/vision/camera/ZeroCopyPicamSource.java
+++ b/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());
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
index cd9c3a6a6..c26f13bce 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AprilTagPipelineSettings.java
+++ b/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;
     }
 
diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
index 1826dde7b..219cc3aa4 100644
--- a/photon-core/src/main/java/org/photonvision/vision/pipeline/CVPipelineSettings.java
+++ b/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
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
index 4f3b369e9..3640403d3 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionModule.java
+++ b/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
diff --git a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java b/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
index 04512dcf3..992c85db5 100644
--- a/photon-core/src/main/java/org/photonvision/vision/processes/VisionSourceSettables.java
+++ b/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")
diff --git a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java b/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
index bb13a57de..813a9d7d9 100644
--- a/photon-core/src/test/java/org/photonvision/vision/processes/VisionModuleManagerTest.java
+++ b/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 {