summaryrefslogtreecommitdiff
path: root/src/ipa/raspberrypi/controller/rpi/af.h
blob: f7baf8979873f52862359f178c936a118c5fd161 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
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