libcamera: software_isp: Add autofocus

Signed-off-by: Vasiliy Doylov <nekocwd@mainlining.org>

include/libcamera/internal/software_isp/swisp_stats.h

@@ -44,6 +44,10 @@ struct SwIspStats {
 	 * \brief A histogram of luminance values
 	 */
 	Histogram yHistogram;
+	/**
+	 * \brief Holds the sharpness of an image
+	 */
+	uint64_t sharpness;
 };
 
 } /* namespace libcamera */

src/ipa/simple/algorithms/af.cpp

@@ -27,14 +27,18 @@ int Af::init(IPAContext &context,
 	     [[maybe_unused]] const YamlObject &tuningData)
 {
 	context.ctrlMap[&controls::LensPosition] = ControlInfo(0.0f, 100.0f, 50.0f);
+	context.ctrlMap[&controls::AfTrigger] = ControlInfo(0, 1, 0);
 	return 0;
 }
 
 int Af::configure(IPAContext &context,
 		  [[maybe_unused]] const IPAConfigInfo &configInfo)
 {
+	context.activeState.knobs.focus_sweep = std::optional<bool>();
 	context.activeState.knobs.focus_pos = std::optional<double>();
-
+	context.activeState.knobs.focus_sweep = false;
+	context.activeState.knobs.focus_pos = 0;
+	context.configuration.focus.skip = 10;
 	return 0;
 }
 
@@ -44,10 +48,24 @@ void Af::queueRequest([[maybe_unused]] typename Module::Context &context,
 		      const ControlList &controls)
 {
 	const auto &focus_pos = controls.get(controls::LensPosition);
+	const auto &af_trigger = controls.get(controls::AfTrigger);
 	if (focus_pos.has_value()) {
 		context.activeState.knobs.focus_pos = focus_pos;
 		LOG(IPASoftAutoFocus, Debug) << "Setting focus position to " << focus_pos.value();
 	}
+	if (af_trigger.has_value()) {
+		context.activeState.knobs.focus_sweep = af_trigger.value() == 1;
+		if(context.activeState.knobs.focus_sweep){
+			context.activeState.knobs.focus_pos = 0;
+			context.configuration.focus.focus_max_pos = 0;
+			context.configuration.focus.sharpness_max = 0;
+			context.configuration.focus.start = 0;
+			context.configuration.focus.stop = 100;
+			context.configuration.focus.step = 25;
+			LOG(IPASoftAutoFocus, Info) << "Starting focus sweep";
+		}
+	}
+
 }
 
 void Af::updateFocus([[maybe_unused]] IPAContext &context, [[maybe_unused]] IPAFrameContext &frameContext, [[maybe_unused]] double exposureMSV)
@@ -55,12 +73,54 @@ void Af::updateFocus([[maybe_unused]] IPAContext &context, [[maybe_unused]] IPAF
 	frameContext.lens.focus_pos = context.activeState.knobs.focus_pos.value_or(50.0) / 100.0 * (context.configuration.focus.focus_max - context.configuration.focus.focus_min);
 }
 
+void Af::step(uint32_t& skip, double& start, double& stop, double& step, double& focus_pos, double& max_pos, uint64_t& max_sharp, uint64_t sharp, bool& sweep){
+	if(!sweep)
+		return;
+	if(skip != 0){
+		skip --;
+		return;
+	}
+	skip = 2;
+	if(focus_pos < start) {
+		focus_pos = start;
+		return;
+	}
+	if(sharp > max_sharp) {
+		max_sharp = sharp;
+		max_pos = focus_pos;
+	}
+	if(focus_pos >= stop) {
+		LOG(IPASoftAutoFocus, Info) << "Best focus on step " <<step << ": " << focus_pos;
+		start = std::clamp(max_pos - step, 0.0, 100.0);
+		stop = std::clamp(max_pos +  step, 0.0, 100.0);
+		focus_pos = start;
+		max_sharp = 0;
+		step /= 2;
+		if(step <= 0.2){
+			sweep = false;
+			LOG(IPASoftAutoFocus, Info) << "Sweep end. Best focus: " << focus_pos;
+		}
+		return;
+	}
+
+	focus_pos += step;
+}
+
 void Af::process([[maybe_unused]] IPAContext &context,
 		 [[maybe_unused]] const uint32_t frame,
 		 [[maybe_unused]] IPAFrameContext &frameContext,
 		 [[maybe_unused]] const SwIspStats *stats,
 		 [[maybe_unused]] ControlList &metadata)
 {
+	step(context.configuration.focus.skip,
+		context.configuration.focus.start,
+		context.configuration.focus.stop,
+		context.configuration.focus.step,
+		context.activeState.knobs.focus_pos.value(),
+		context.configuration.focus.focus_max_pos,
+		context.configuration.focus.sharpness_max,
+		stats->sharpness,
+		context.activeState.knobs.focus_sweep.value());
 	updateFocus(context, frameContext, 0);
 }
 

src/ipa/simple/algorithms/af.h

@@ -33,6 +33,7 @@ public:
 
 private:
 	void updateFocus(IPAContext &context, IPAFrameContext &frameContext, double focus);
+	void step(uint32_t& skip, double& start, double& stop, double& step, double& focus_pos, double& max_pos, uint64_t& max_sharp, uint64_t sharp, bool& sweep);
 };
 
 } /* namespace ipa::soft::algorithms */

src/ipa/simple/ipa_context.h

@@ -36,6 +36,10 @@ struct IPASessionConfiguration {
 	} black;
 	struct {
 		int32_t focus_min, focus_max;
+		double focus_max_pos;
+		uint64_t sharpness_max;
+		double start, stop, step;
+		uint32_t skip;
 	} focus;
 };
 
@@ -77,6 +81,8 @@ struct IPAActiveState {
 		std::optional<double> focus_pos;
 		/* 0..1 range, 1 = normal */
 		std::optional<bool> stats_enabled;
+		/* 0..1 range, 0 = normal */
+		std::optional<bool> focus_sweep;
 	} knobs;
 };
 

src/libcamera/software_isp/swstats_cpu.cpp

@@ -147,7 +147,10 @@ static constexpr unsigned int kBlueYMul = 29; /* 0.114 * 256 */
                                           \
 	uint64_t sumR = 0;                \
 	uint64_t sumG = 0;                \
-	uint64_t sumB = 0;
+	uint64_t sumB = 0;                \
+	pixel_t r0 = 0, r1 = 0, b0 = 0,   \
+	        b1 = 0, g0 = 0, g1 = 0;   \
+	uint64_t sharpness = 0;
 
 #define SWSTATS_ACCUMULATE_LINE_STATS(div) \
 	sumR += r;                         \
@@ -157,12 +160,18 @@ static constexpr unsigned int kBlueYMul = 29; /* 0.114 * 256 */
 	yVal = r * kRedYMul;               \
 	yVal += g * kGreenYMul;            \
 	yVal += b * kBlueYMul;             \
-	stats_.yHistogram[yVal * SwIspStats::kYHistogramSize / (256 * 256 * (div))]++;
+	stats_.yHistogram[yVal * SwIspStats::kYHistogramSize / (256 * 256 * (div))]++; \
+	if (r0 != 0)                       \
+		sharpness += abs(r - 2*r1 + r0) * kRedYMul + abs(g - 2*g1 + g0) * kGreenYMul + abs(b - 2*b1 + b0) * kBlueYMul; \
+	r0 = r1; g0 = g1; b0 = b1; \
+	r1 = r; g1 = g; b1 = b; \
+
 
 #define SWSTATS_FINISH_LINE_STATS() \
 	stats_.sumR_ += sumR;       \
 	stats_.sumG_ += sumG;       \
-	stats_.sumB_ += sumB;
+	stats_.sumB_ += sumB;	    \
+	stats_.sharpness += sharpness;
 
 void SwStatsCpu::statsBGGR8Line0(const uint8_t *src[])
 {
@@ -306,6 +315,7 @@ void SwStatsCpu::startFrame(void)
 	stats_.sumR_ = 0;
 	stats_.sumB_ = 0;
 	stats_.sumG_ = 0;
+	stats_.sharpness = 0;
 	stats_.yHistogram.fill(0);
 }