From cc010b0c35bfa0eee0bafba6a5803d1405542456 Mon Sep 17 00:00:00 2001 From: Nick Hollinghurst Date: Mon, 23 Jan 2023 15:49:31 +0000 Subject: 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 Signed-off-by: Naushir Patuck Reviewed-by: Naushir Patuck Reviewed-by: David Plowman Signed-off-by: Kieran Bingham --- src/ipa/raspberrypi/controller/rpi/af.h | 156 ++++++++++++++++++++++++++++++++ 1 file changed, 156 insertions(+) create mode 100644 src/ipa/raspberrypi/controller/rpi/af.h (limited to 'src/ipa/raspberrypi/controller/rpi/af.h') 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 ¶ms) 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 const &wins) override; + void setMode(AfMode mode) override; + AfMode getMode() const override; + bool setLensPosition(double dioptres, int32_t *hwpos) override; + std::optional 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 ¶ms); + }; + + 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 ¶ms); + }; + + 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 ¶ms); + 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 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 scanData_; + AfState reportState_; +}; + +} // namespace RPiController -- cgit v1.2.1