001/*
002 * Copyright (C) Photon Vision.
003 *
004 * This program is free software: you can redistribute it and/or modify
005 * it under the terms of the GNU General Public License as published by
006 * the Free Software Foundation, either version 3 of the License, or
007 * (at your option) any later version.
008 *
009 * This program is distributed in the hope that it will be useful,
010 * but WITHOUT ANY WARRANTY; without even the implied warranty of
011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
012 * GNU General Public License for more details.
013 *
014 * You should have received a copy of the GNU General Public License
015 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
016 */
017
018package org.photonvision.vision.camera.csi;
019
020import edu.wpi.first.cscore.VideoMode;
021import edu.wpi.first.math.MathUtil;
022import edu.wpi.first.math.Pair;
023import edu.wpi.first.util.PixelFormat;
024import java.util.HashMap;
025import org.photonvision.common.configuration.CameraConfiguration;
026import org.photonvision.common.util.math.MathUtils;
027import org.photonvision.raspi.LibCameraJNI;
028import org.photonvision.vision.camera.csi.LibcameraGpuSource.FPSRatedVideoMode;
029import org.photonvision.vision.opencv.ImageRotationMode;
030import org.photonvision.vision.processes.VisionSourceSettables;
031
032public class LibcameraGpuSettables extends VisionSourceSettables {
033    private FPSRatedVideoMode currentVideoMode;
034    private double lastManualExposure = 50;
035    private int lastBrightness = 50;
036    private boolean lastAutoExposureActive;
037    private int lastGain = 50;
038    private Pair<Integer, Integer> lastAwbGains = new Pair<>(18, 18);
039    public long r_ptr = 0;
040
041    private final LibCameraJNI.SensorModel sensorModel;
042
043    private double minExposure = 1;
044    private double maxExposure = 80000;
045
046    private ImageRotationMode m_rotationMode = ImageRotationMode.DEG_0;
047
048    public final Object CAMERA_LOCK = new Object();
049
050    public void setRotation(ImageRotationMode rotationMode) {
051        if (rotationMode != m_rotationMode) {
052            m_rotationMode = rotationMode;
053
054            setVideoModeInternal(getCurrentVideoMode());
055        }
056    }
057
058    public ImageRotationMode getRotation() {
059        return m_rotationMode;
060    }
061
062    public LibcameraGpuSettables(CameraConfiguration configuration) {
063        super(configuration);
064
065        videoModes = new HashMap<>();
066
067        sensorModel = LibCameraJNI.getSensorModel(configuration.matchedCameraInfo.path());
068
069        if (sensorModel == LibCameraJNI.SensorModel.IMX219) {
070            // Settings for the IMX219 sensor, which is used on the Pi Camera Module v2
071            videoModes.put(0, new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 120, 120, .39));
072            videoModes.put(1, new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 30, 30, .39));
073            videoModes.put(2, new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 65, 90, .39));
074            videoModes.put(3, new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 30, 30, .39));
075            // TODO: fix 1280x720 in the native code and re-add it
076            videoModes.put(4, new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, .53));
077            videoModes.put(5, new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 2, 2464 / 2, 15, 20, 1));
078            videoModes.put(6, new FPSRatedVideoMode(PixelFormat.kUnknown, 3280 / 4, 2464 / 4, 15, 20, 1));
079        } else if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
080            videoModes.put(0, new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 30, 30, .39));
081            videoModes.put(1, new FPSRatedVideoMode(PixelFormat.kUnknown, 1280 / 2, 800 / 2, 60, 60, 1));
082            videoModes.put(2, new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 65, 90, .39));
083            videoModes.put(3, new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 800, 60, 60, 1));
084
085        } else {
086            if (sensorModel == LibCameraJNI.SensorModel.IMX477) {
087                LibcameraGpuSource.logger.warn(
088                        "It appears you are using a Pi HQ Camera. This camera is not officially supported. You will have to set your camera FOV differently based on resolution.");
089            } else if (sensorModel == LibCameraJNI.SensorModel.IMX708) {
090                LibcameraGpuSource.logger.warn(
091                        "It appears you are using a Pi Camera V3. This camera is not officially supported. You will have to set your camera FOV differently based on resolution.");
092            } else if (sensorModel == LibCameraJNI.SensorModel.Unknown) {
093                LibcameraGpuSource.logger.warn(
094                        "You have an unknown sensor connected to your Pi over CSI! This is likely a bug. If it is not, then you will have to set your camera FOV differently based on resolution.");
095            }
096
097            // Settings for the OV5647 sensor, which is used by the Pi Camera Module v1
098            videoModes.put(0, new FPSRatedVideoMode(PixelFormat.kUnknown, 320, 240, 90, 90, 1));
099            videoModes.put(1, new FPSRatedVideoMode(PixelFormat.kUnknown, 640, 480, 85, 90, 1));
100            videoModes.put(2, new FPSRatedVideoMode(PixelFormat.kUnknown, 960, 720, 45, 49, 0.74));
101            // Half the size of the active areas on the OV5647
102            videoModes.put(3, new FPSRatedVideoMode(PixelFormat.kUnknown, 2592 / 2, 1944 / 2, 20, 20, 1));
103            videoModes.put(4, new FPSRatedVideoMode(PixelFormat.kUnknown, 1280, 720, 30, 45, 0.91));
104            videoModes.put(5, new FPSRatedVideoMode(PixelFormat.kUnknown, 1920, 1080, 15, 20, 0.72));
105        }
106
107        // TODO need to add more video modes for new sensors here
108
109        currentVideoMode = (FPSRatedVideoMode) videoModes.get(0);
110
111        if (sensorModel == LibCameraJNI.SensorModel.OV9281) {
112            minExposure = 7;
113        } else if (sensorModel == LibCameraJNI.SensorModel.OV5647) {
114            minExposure = 560;
115        }
116        this.cameraPropertiesCached =
117                true; // Camera properties are not able to be changed so they are always cached
118    }
119
120    @Override
121    public double getFOV() {
122        return getCurrentVideoMode().fovMultiplier * getConfiguration().FOV;
123    }
124
125    @Override
126    public void setAutoExposure(boolean cameraAutoExposure) {
127        logger.debug("Setting auto exposure to " + cameraAutoExposure);
128        lastAutoExposureActive = cameraAutoExposure;
129        LibCameraJNI.setAutoExposure(r_ptr, cameraAutoExposure);
130        if (!cameraAutoExposure) {
131            setExposureRaw(lastManualExposure);
132        }
133    }
134
135    @Override
136    public void setExposureRaw(double exposureRaw) {
137        logger.debug("Setting exposure to " + exposureRaw);
138
139        LibCameraJNI.setAutoExposure(r_ptr, false);
140
141        // Store the exposure for use when we need to recreate the camera.
142        lastManualExposure = exposureRaw;
143
144        // 80,000 uS seems like an exposure value that will be greater than ever needed while giving
145        // enough control over exposure.
146        exposureRaw = MathUtil.clamp(exposureRaw, minExposure, maxExposure);
147
148        var success = LibCameraJNI.setExposure(r_ptr, (int) exposureRaw);
149        if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera exposure");
150    }
151
152    @Override
153    public void setBrightness(int brightness) {
154        lastBrightness = brightness;
155        double realBrightness = MathUtils.map(brightness, 0.0, 100.0, -1.0, 1.0);
156        var success = LibCameraJNI.setBrightness(r_ptr, realBrightness);
157        if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera brightness");
158    }
159
160    @Override
161    public void setGain(int gain) {
162        lastGain = gain;
163
164        // Map and clamp gain to values between 1 and 10 (libcamera min and gain that just seems higher
165        // than ever needed) from 0 to 100 (UI values).
166        var success =
167                LibCameraJNI.setAnalogGain(
168                        r_ptr, MathUtil.clamp(MathUtils.map(gain, 0.0, 100.0, 1.0, 10.0), 1.0, 10.0));
169        if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera gain");
170    }
171
172    @Override
173    public void setRedGain(int red) {
174        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
175            lastAwbGains = Pair.of(red, lastAwbGains.getSecond());
176            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
177        }
178    }
179
180    @Override
181    public void setBlueGain(int blue) {
182        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
183            lastAwbGains = Pair.of(lastAwbGains.getFirst(), blue);
184            setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
185        }
186    }
187
188    public void setAwbGain(int red, int blue) {
189        if (sensorModel != LibCameraJNI.SensorModel.OV9281) {
190            var success = LibCameraJNI.setAwbGain(r_ptr, red / 10.0, blue / 10.0);
191            if (!success) LibcameraGpuSource.logger.warn("Couldn't set Pi Camera AWB gains");
192        }
193    }
194
195    @Override
196    public FPSRatedVideoMode getCurrentVideoMode() {
197        return currentVideoMode;
198    }
199
200    @Override
201    protected void setVideoModeInternal(VideoMode videoMode) {
202        var mode = (FPSRatedVideoMode) videoMode;
203
204        // We need to make sure that other threads don't try to do anything funny while we're recreating
205        // the camera
206        synchronized (CAMERA_LOCK) {
207            if (r_ptr != 0) {
208                logger.debug("Stopping libcamera");
209                if (!LibCameraJNI.stopCamera(r_ptr)) {
210                    logger.error("Couldn't stop a zero copy Pi Camera while switching video modes");
211                }
212                logger.debug("Destroying libcamera");
213                if (!LibCameraJNI.destroyCamera(r_ptr)) {
214                    logger.error("Couldn't destroy a zero copy Pi Camera while switching video modes");
215                }
216            }
217
218            logger.debug("Creating libcamera");
219            r_ptr =
220                    LibCameraJNI.createCamera(
221                            getConfiguration().matchedCameraInfo.path(),
222                            mode.width,
223                            mode.height,
224                            (m_rotationMode == ImageRotationMode.DEG_180_CCW ? 180 : 0));
225            if (r_ptr == 0) {
226                logger.error("Couldn't create a zero copy Pi Camera while switching video modes");
227                if (!LibCameraJNI.destroyCamera(r_ptr)) {
228                    logger.error("Couldn't destroy a zero copy Pi Camera while switching video modes");
229                }
230            }
231            logger.debug("Starting libcamera");
232            if (!LibCameraJNI.startCamera(r_ptr)) {
233                logger.error("Couldn't start a zero copy Pi Camera while switching video modes");
234            }
235        }
236
237        // We don't store last settings on the native side, and when you change video mode these get
238        // reset on the native driver's end. Reset em back to be correct
239        setExposureRaw(lastManualExposure);
240        setAutoExposure(lastAutoExposureActive);
241        setBrightness(lastBrightness);
242        setGain(lastGain);
243        setAwbGain(lastAwbGains.getFirst(), lastAwbGains.getSecond());
244
245        LibCameraJNI.setFramesToCopy(r_ptr, true, true);
246
247        currentVideoMode = mode;
248    }
249
250    @Override
251    public HashMap<Integer, VideoMode> getAllVideoModes() {
252        return videoModes;
253    }
254
255    public LibCameraJNI.SensorModel getModel() {
256        return sensorModel;
257    }
258
259    @Override
260    public double getMinExposureRaw() {
261        return this.minExposure;
262    }
263
264    @Override
265    public double getMaxExposureRaw() {
266        return this.maxExposure;
267    }
268
269    @Override
270    public void setAutoWhiteBalance(boolean autowb) {
271        throw new RuntimeException("Libcamera doesn't support AWB temp (yet)");
272    }
273
274    @Override
275    public void setWhiteBalanceTemp(double temp) {
276        throw new RuntimeException("Libcamera doesn't support AWB temp -- use red/blue gain instead");
277    }
278
279    @Override
280    public double getMaxWhiteBalanceTemp() {
281        return 2;
282    }
283
284    @Override
285    public double getMinWhiteBalanceTemp() {
286        return 1;
287    }
288}