ipa: raspberrypi: First version of autofocus algorithm using PDAF

Provide the first version of the Raspberry Pi autofocus algorithm. This
implementation uses a hybrid of contrast detect autofocus (CDAF) and phase
detect autofocus (PDAF) statistics. PDAF is always preferred over CDAF due to
having less "hunting" behavior.

Signed-off-by: Nick Hollinghurst <nick.hollinghurst@raspberrypi.com>
Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: David Plowman <david.plowman@raspberrypi.com>
Signed-off-by: Kieran Bingham <kieran.bingham@ideasonboard.com>

1 files changed, 156 insertions, 0 deletions

diff --git a/src/ipa/raspberrypi/controller/rpi/af.h b/src/ipa/raspberrypi/controller/rpi/af.h
new file mode 100644
index 00000000..f7baf897
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/rpi/af.h
@@ -0,0 +1,156 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2022-2023, Raspberry Pi Ltd
+ *
+ * af.h - Autofocus control algorithm
+ */
+#pragma once
+
+#include "../af_algorithm.h"
+#include "../af_status.h"
+#include "../pdaf_data.h"
+#include "../pwl.h"
+
+/*
+ * This algorithm implements a hybrid of CDAF and PDAF, favouring PDAF.
+ *
+ * Whenever PDAF is available, it is used in a continuous feedback loop.
+ * When triggered in auto mode, we simply enable AF for a limited number
+ * of frames (it may terminate early if the delta becomes small enough).
+ *
+ * When PDAF confidence is low (due e.g. to low contrast or extreme defocus)
+ * or PDAF data are absent, fall back to CDAF with a programmed scan pattern.
+ * A coarse and fine scan are performed, using ISP's CDAF focus FoM to
+ * estimate the lens position with peak contrast. This is slower due to
+ * extra latency in the ISP, and requires a settling time between steps.
+ *
+ * Some hysteresis is applied to the switch between PDAF and CDAF, to avoid
+ * "nuisance" scans. During each interval where PDAF is not working, only
+ * ONE scan will be performed; CAF cannot track objects using CDAF alone.
+ *
+ * This algorithm is unrelated to "rpi.focus" which merely reports CDAF FoM.
+ */
+
+namespace RPiController {
+
+class Af : public AfAlgorithm
+{
+public:
+	Af(Controller *controller = NULL);
+	~Af();
+	char const *name() const override;
+	int read(const libcamera::YamlObject &params) override;
+	void initialise() override;
+
+	/* IPA calls */
+	void switchMode(CameraMode const &cameraMode, Metadata *metadata) override;
+	void prepare(Metadata *imageMetadata) override;
+	void process(StatisticsPtr &stats, Metadata *imageMetadata) override;
+
+	/* controls */
+	void setRange(AfRange range) override;
+	void setSpeed(AfSpeed speed) override;
+	void setMetering(bool use_windows) override;
+	void setWindows(libcamera::Span<libcamera::Rectangle const> const &wins) override;
+	void setMode(AfMode mode) override;
+	AfMode getMode() const override;
+	bool setLensPosition(double dioptres, int32_t *hwpos) override;
+	std::optional<double> getLensPosition() const override;
+	void triggerScan() override;
+	void cancelScan() override;
+	void pause(AfPause pause) override;
+
+private:
+	enum class ScanState {
+		Idle = 0,
+		Trigger,
+		Pdaf,
+		Coarse,
+		Fine,
+		Settle
+	};
+
+	struct RangeDependentParams {
+		double focusMin;       		/* lower (far) limit in dipotres */
+		double focusMax;	       	/* upper (near) limit in dioptres */
+		double focusDefault;		/* default setting ("hyperfocal") */
+
+		RangeDependentParams();
+		void read(const libcamera::YamlObject &params);
+	};
+
+	struct SpeedDependentParams {
+		double stepCoarse;		/* used for scans */
+		double stepFine;		/* used for scans */
+		double contrastRatio;		/* used for scan termination and reporting */
+		double pdafGain;		/* coefficient for PDAF feedback loop */
+		double pdafSquelch;		/* PDAF stability parameter (device-specific) */
+		double maxSlew;			/* limit for lens movement per frame */
+		uint32_t pdafFrames;		/* number of iterations when triggered */
+		uint32_t dropoutFrames;		/* number of non-PDAF frames to switch to CDAF */
+		uint32_t stepFrames;		/* frames to skip in between steps of a scan */
+
+		SpeedDependentParams();
+		void read(const libcamera::YamlObject &params);
+	};
+
+	struct CfgParams {
+		RangeDependentParams ranges[AfRangeMax];
+		SpeedDependentParams speeds[AfSpeedMax];
+		uint32_t confEpsilon;	       	/* PDAF hysteresis threshold (sensor-specific) */
+		uint32_t confThresh;	       	/* PDAF confidence cell min (sensor-specific) */
+		uint32_t confClip;	       	/* PDAF confidence cell max (sensor-specific) */
+		uint32_t skipFrames;	       	/* frames to skip at start or modeswitch */
+		Pwl map;		       	/* converts dioptres -> lens driver position */
+
+		CfgParams();
+		int read(const libcamera::YamlObject &params);
+		void initialise();
+	};
+
+	struct ScanRecord {
+		double focus;
+		double contrast;
+		double phase;
+		double conf;
+	};
+
+	void computeWeights();
+	bool getPhase(PdafData const &data, double &phase, double &conf) const;
+	double getContrast(struct bcm2835_isp_stats_focus const focus_stats[FOCUS_REGIONS]) const;
+	void doPDAF(double phase, double conf);
+	bool earlyTerminationByPhase(double phase);
+	double findPeak(unsigned index) const;
+	void doScan(double contrast, double phase, double conf);
+	void doAF(double contrast, double phase, double conf);
+	void updateLensPosition();
+	void startAF();
+	void startProgrammedScan();
+	void goIdle();
+
+	/* Configuration and settings */
+	CfgParams cfg_;
+	AfRange range_;
+	AfSpeed speed_;
+	AfMode mode_;
+	bool pauseFlag_;
+	libcamera::Rectangle statsRegion_;
+	std::vector<libcamera::Rectangle> windows_;
+	bool useWindows_;
+	uint8_t phaseWeights_[PDAF_DATA_ROWS][PDAF_DATA_COLS];
+	uint16_t contrastWeights_[FOCUS_REGIONS];
+	uint32_t sumWeights_;
+
+	/* Working state. */
+	ScanState scanState_;
+	bool initted_;
+	double ftarget_, fsmooth_;
+	double prevContrast_;
+	unsigned skipCount_, stepCount_, dropCount_;
+	unsigned scanMaxIndex_;
+	double scanMaxContrast_, scanMinContrast_;
+	std::vector<ScanRecord> scanData_;
+	AfState reportState_;
+};
+
+} // namespace RPiController