5 files changed, 113 insertions, 0 deletions

diff --git a/src/ipa/raspberrypi/controller/focus_status.h b/src/ipa/raspberrypi/controller/focus_status.h
new file mode 100644
index 00000000..3ad88777
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/focus_status.h
@@ -0,0 +1,26 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, Raspberry Pi (Trading) Limited
+ *
+ * focus_status.h - focus measurement status
+ */
+#pragma once
+
+#include <linux/bcm2835-isp.h>
+
+// The focus algorithm should post the following structure into the image's
+// "focus.status" metadata. Recall that it's only reporting focus (contrast)
+// measurements, it's not driving any kind of auto-focus algorithm!
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct FocusStatus {
+	int num;
+	uint32_t focus_measures[FOCUS_REGIONS];
+};
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/ipa/raspberrypi/controller/rpi/focus.cpp b/src/ipa/raspberrypi/controller/rpi/focus.cpp
new file mode 100644
index 00000000..1e2b6491
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/rpi/focus.cpp
@@ -0,0 +1,51 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, Raspberry Pi (Trading) Limited
+ *
+ * focus.cpp - focus algorithm
+ */
+#include <stdint.h>
+
+#include "../focus_status.h"
+#include "../logging.hpp"
+#include "focus.hpp"
+
+using namespace RPi;
+
+#define NAME "rpi.focus"
+
+Focus::Focus(Controller *controller)
+	: Algorithm(controller)
+{
+}
+
+char const *Focus::Name() const
+{
+	return NAME;
+}
+
+void Focus::Read(boost::property_tree::ptree const &params)
+{
+	print_ = params.get<int>("print", 0);
+}
+
+void Focus::Process(StatisticsPtr &stats, Metadata *image_metadata)
+{
+	FocusStatus status;
+	unsigned int i;
+	for (i = 0; i < FOCUS_REGIONS; i++)
+		status.focus_measures[i] = stats->focus_stats[i].contrast_val[1][1] / 1000;
+	status.num = i;
+	image_metadata->Set("focus.status", status);
+	if (print_) {
+		uint32_t value = (status.focus_measures[5] + status.focus_measures[6]) / 10;
+		RPI_LOG("Focus contrast measure: " << value);
+	}
+}
+
+/* Register algorithm with the system. */
+static Algorithm *Create(Controller *controller)
+{
+	return new Focus(controller);
+}
+static RegisterAlgorithm reg(NAME, &Create);
diff --git a/src/ipa/raspberrypi/controller/rpi/focus.hpp b/src/ipa/raspberrypi/controller/rpi/focus.hpp
new file mode 100644
index 00000000..d53401f7
--- /dev/null
+++ b/src/ipa/raspberrypi/controller/rpi/focus.hpp
@@ -0,0 +1,31 @@
+/* SPDX-License-Identifier: BSD-2-Clause */
+/*
+ * Copyright (C) 2020, Raspberry Pi (Trading) Limited
+ *
+ * focus.hpp - focus algorithm
+ */
+#pragma once
+
+#include "../algorithm.hpp"
+#include "../metadata.hpp"
+
+/*
+ * The "focus" algorithm. All it does it print out a version of the
+ * focus contrast measure; there is no actual auto-focus mechanism to
+ * control.
+ */
+
+namespace RPi {
+
+class Focus : public Algorithm
+{
+public:
+	Focus(Controller *controller);
+	char const *Name() const override;
+	void Read(boost::property_tree::ptree const &params) override;
+	void Process(StatisticsPtr &stats, Metadata *image_metadata) override;
+private:
+	bool print_;
+};
+
+} /* namespace RPi */
diff --git a/src/ipa/raspberrypi/data/imx477.json b/src/ipa/raspberrypi/data/imx477.json
index dce5234f..389e8ce8 100644
--- a/src/ipa/raspberrypi/data/imx477.json
+++ b/src/ipa/raspberrypi/data/imx477.json
@@ -412,5 +412,9 @@
     "rpi.sharpen":
     {
 
+    },
+    "rpi.focus":
+    {
+	"print": 1
     }
 }
diff --git a/src/ipa/raspberrypi/meson.build b/src/ipa/raspberrypi/meson.build
index 697902e9..9445cd09 100644
--- a/src/ipa/raspberrypi/meson.build
+++ b/src/ipa/raspberrypi/meson.build
@@ -29,6 +29,7 @@ rpi_ipa_sources = files([
     'controller/rpi/awb.cpp',
     'controller/rpi/sharpen.cpp',
     'controller/rpi/black_level.cpp',
+    'controller/rpi/focus.cpp',
     'controller/rpi/geq.cpp',
     'controller/rpi/noise.cpp',
     'controller/rpi/lux.cpp',