sensor: afbr_s50: Add private channel to obtain pixel matrix

Each time this sensor gets a reading, it contains a matrix of 4 x 32
pixels containing distance readings, from which the 1-D result is
calculated. The private channel would expose this array through
Sensor APIs.

Signed-off-by: Luis Ubieda <[email protected]>

Author: ubieda

drivers/sensor/broadcom/afbr_s50/afbr_s50.c

@@ -15,6 +15,8 @@
 #include <zephyr/rtio/rtio.h>
 #include <zephyr/rtio/work.h>
 
+#include <zephyr/drivers/sensor/afbr_s50.h>
+
 #include <api/argus_api.h>
 #include <platform/argus_irq.h>
 
@@ -83,7 +85,8 @@ static void data_ready_work_handler(struct rtio_iodev_sqe *iodev_sqe)
 	}
 
 	edata->header.timestamp = sensor_clock_cycles_to_ns(cycles);
-	edata->header.channels = afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE);
+	edata->header.channels = afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE) |
+				 afbr_s50_encode_channel(SENSOR_CHAN_AFBR_S50_PIXELS);
 	edata->header.events = cfg->is_streaming ? afbr_s50_encode_event(SENSOR_TRIG_DATA_READY) :
 						   0;
 

drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.c

@@ -9,6 +9,7 @@
 
 #include <zephyr/drivers/sensor_clock.h>
 #include <api/argus_res.h>
+#include <zephyr/drivers/sensor/afbr_s50.h>
 
 #include "afbr_s50_decoder.h"
 
@@ -20,6 +21,8 @@ uint8_t afbr_s50_encode_channel(uint16_t chan)
 	switch (chan) {
 	case SENSOR_CHAN_DISTANCE:
 		return BIT(0);
+	case SENSOR_CHAN_AFBR_S50_PIXELS:
+		return BIT(1);
 	default:
 		return 0;
 	}
@@ -46,7 +49,8 @@ static int afbr_s50_decoder_get_frame_count(const uint8_t *buffer,
 
 	switch (chan_spec.chan_type) {
 	case SENSOR_CHAN_DISTANCE:
-		if (edata->header.channels & afbr_s50_encode_channel(SENSOR_CHAN_DISTANCE)) {
+	case SENSOR_CHAN_AFBR_S50_PIXELS:
+		if (edata->header.channels & afbr_s50_encode_channel(chan_spec.chan_type)) {
 			*frame_count = 1;
 			return 0;
 		}
@@ -68,6 +72,11 @@ static int afbr_s50_decoder_get_size_info(struct sensor_chan_spec chan_spec,
 		*base_size = sizeof(struct sensor_q31_data);
 		*frame_size = sizeof(struct sensor_q31_sample_data);
 		return 0;
+	case SENSOR_CHAN_AFBR_S50_PIXELS:
+		*base_size = sizeof(struct sensor_q31_data) +
+			     31 * sizeof(struct sensor_q31_sample_data);
+		*frame_size = 32 * sizeof(struct sensor_q31_sample_data);
+		return 0;
 	default:
 		return -ENOTSUP;
 	}
@@ -107,6 +116,36 @@ static int afbr_s50_decoder_decode(const uint8_t *buffer,
 		*fit = 1;
 		return 1;
 	}
+	case SENSOR_CHAN_AFBR_S50_PIXELS: {
+		struct sensor_q31_data *out = data_out;
+
+		if ((edata->header.channels &
+		     afbr_s50_encode_channel(SENSOR_CHAN_AFBR_S50_PIXELS)) == 0) {
+			return -ENODATA;
+		}
+
+		out->header.base_timestamp_ns = edata->header.timestamp;
+		out->header.reading_count = 32;
+		/* Result comes encoded in Q9.22 format */
+		out->shift = 9;
+
+		for (size_t i = 0 ; i < 32 ; i++) {
+			if (edata->payload.Pixels[i].Amplitude == 0xFFFF ||
+			    edata->payload.Pixels[i].Status != PIXEL_OK) {
+				LOG_DBG("Invalid pixel: %zu, Amplitude: %u, Status: %u", i,
+					edata->payload.Pixels[i].Amplitude,
+					edata->payload.Pixels[i].Status);
+
+				out->readings[i].value = AFBR_PIXEL_INVALID_VALUE;
+			} else {
+				out->readings[i].value = edata->payload.Pixels[i].Range;
+			}
+			out->readings[i].timestamp_delta = 0;
+		}
+
+		*fit = 1;
+		return 1;
+	}
 	default:
 		return -EINVAL;
 	}

drivers/sensor/broadcom/afbr_s50/afbr_s50_decoder.h

@@ -16,7 +16,7 @@
 struct afbr_s50_edata {
 	struct {
 		uint64_t timestamp;
-		uint8_t channels : 1;
+		uint8_t channels : 2;
 		uint8_t events : 1;
 	} header;
 	argus_results_t payload;

include/zephyr/drivers/sensor/afbr_s50.h

@@ -0,0 +1,31 @@
+/*
+ * Copyright (c) 2025 Croxel Inc.
+ * Copyright (c) 2025 CogniPilot Foundation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef ZEPHYR_INCLUDE_DRIVERS_SENSOR_AFBR_S50_H_
+#define ZEPHYR_INCLUDE_DRIVERS_SENSOR_AFBR_S50_H_
+
+#include <zephyr/drivers/sensor.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** Disregard pixel reading if contains this value */
+#define AFBR_PIXEL_INVALID_VALUE 0x80000000
+
+/** Private sensor channel to obtain matrix of pixels with readings in meters.
+ * This sensor supports up to 32 pixels in a single reading (4 x 8 matrix).
+ */
+enum sensor_channel_afbr_s50 {
+	SENSOR_CHAN_AFBR_S50_PIXELS = SENSOR_CHAN_PRIV_START + 1,
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* ZEPHYR_INCLUDE_DRIVERS_SENSOR_AFBR_S50_H_ */