ipa: rpi: Add "focus" algorithm

Adds FocusStatus to the image metadata, containing contrast measurements
across the image. Optionally also prints a contrast measure to the
console, to aid in manual adjustment of the lens. Note that it is not an
actual auto-focus algorithm that can drive a lens!

Signed-off-by: David Plowman <david.plowman@raspberrypi.com>
Signed-off-by: Naushir Patuck <naush@raspberrypi.com>
Reviewed-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

3 files changed, 108 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 */