diff --git a/boards/ark/can-flow/firmware.prototype b/boards/ark/can-flow/firmware.prototype
index a59e1adca629..fc0df258228e 100644
--- a/boards/ark/can-flow/firmware.prototype
+++ b/boards/ark/can-flow/firmware.prototype
@@ -4,7 +4,7 @@
     "description": "Firmware for the ARK flow board",
     "image": "",
     "build_time": 0,
-    "summary": "ARFFLOW",
+    "summary": "ARKFLOW",
     "version": "0.1",
     "image_size": 0,
     "image_maxsize": 2080768,
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h
index 44cdc921683d..3e30649ae104 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h
@@ -160,6 +160,11 @@ status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t m
  *          Also refer to #Argus_ReinitMode, which uses a specified measurement
  *          mode instead of the currently active measurement mode.
  *
+ * @note    If a full re-initialization is not desired, refer to the
+ *          #Argus_RestoreDeviceState function that will only re-write the
+ *          register map to the device to restore its state after an power
+ *          cycle.
+ *
  * @param   hnd The API handle; contains all internal states and data.
  *
  * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
@@ -182,6 +187,11 @@ status_t Argus_Reinit(argus_hnd_t *hnd);
  *          Also refer to #Argus_Reinit, which re-uses the currently active
  *          measurement mode instead of an user specified measurement mode.
  *
+ * @note    If a full re-initialization is not desired, refer to the
+ *          #Argus_RestoreDeviceState function that will only re-write the
+ *          register map to the device to restore its state after an power
+ *          cycle.
+ *
  * @param   hnd The API handle; contains all internal states and data.
  *
  * @param   mode The specified measurement mode to be initialized.
@@ -274,6 +284,69 @@ argus_hnd_t *Argus_CreateHandle(void);
  *****************************************************************************/
 status_t Argus_DestroyHandle(argus_hnd_t *hnd);
 
+/*!***************************************************************************
+ * @brief   Restores the device state with a re-write of all register values.
+ *
+ * @details The function invalidates and restores the device state by executing
+ *          a re-write of the full register map.
+ *
+ *          The purpose of this function is to recover from known external
+ *          events like power cycles, for example due to sleep / wake-up
+ *          functionality. This can be implemented by cutting off the external
+ *          power supply of the device (e.g. via a MOSFET switch controlled by
+ *          a GPIB pin). By calling this function, the expected state of the
+ *          API is written to the device without the need to fully re-initialize
+ *          the device. Thus, the API can resume where it has stopped as if
+ *          there has never been a power cycle.
+ *
+ *          The internal state machines like the dynamic configuration adaption
+ *          (DCA) algorithm will not be reseted. The API/sensor will immediately
+ *          resume at the last state that was optimized for the given
+ *          environmental conditions.
+ *
+ *          The use case of sleep / wake-up can be implemented as follows:
+ *
+ *          1. In case of ongoing measurements, stop the measurements via
+ *             the #Argus_StopMeasurementTimer function (if started by the
+ *             #Argus_StartMeasurementTimer function).
+ *
+ *          2. Shut down the device by removing the 5V power supply, e.g.
+ *             via a GPIO pin that switches a MOSFET circuit.
+ *
+ *          3. After the desired sleep period, power the device by switching
+ *             the 5V power supply on again. Wait until the power-on-reset
+ *             (POR) is finished (approx. 1 ms) or just repeat step 4 until
+ *             it succeeds.
+ *
+ *          4. Call the #Argus_RestoreDeviceState function to trigger the
+ *             restoration of the device state in the API. Note that the
+ *             function will return an error code if it fails. One can repeat
+ *             the execution of that function a few times until it succeeds.
+ *
+ *          6. Continue with measurements via #Argus_StartMeasurementTimer
+ *             of #Argus_TriggerMeasurement functions as desired.
+ *
+ * @note    If a complete re-initialization (= soft-reset) is desired, see
+ *          the #Argus_Reinit functionality.
+ *
+ * @note    Changing a configuration or calibration parameter will always
+ *          invalidate the device state as well as the state machine of the
+ *          dynamic configuration adaption (DCA) algorithm. In that case, the
+ *          device/API needs a few measurements to adopt to the present
+ *          environmental conditions before the first valid measurement result
+ *          can be obtained. This is almost similar to re-initializing the
+ *          device (see #Argus_Reinit) which would also re-read the EEPROM.
+ *          On the other hand, the #Argus_RestoreDeviceState does not reset
+ *          or re-initialize anything. It just makes sure that the device
+ *          register map (which has changed to its reset values after the
+ *          power cycle) is what the API expects upon the next measurement.
+ *
+ * @param   hnd The device handle object to be invalidated.
+ *
+ * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
+ *****************************************************************************/
+status_t Argus_RestoreDeviceState(argus_hnd_t *hnd);
+
 /*!**************************************************************************
  * Generic API
  ****************************************************************************/
@@ -726,7 +799,7 @@ status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd);
  *          After calibration has finished successfully, the obtained data is
  *          applied immediately and can be read from the API using the
  *          #Argus_GetCalibrationPixelRangeOffsets or
- *          #Argus_GetCalibrationGlobalRangeOffset function.
+ *          #Argus_GetCalibrationGlobalRangeOffsets function.
  *
  * @param   hnd The API handle; contains all internal states and data.
  * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
@@ -775,7 +848,7 @@ status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd);
  *          After calibration has finished successfully, the obtained data is
  *          applied immediately and can be read from the API using the
  *          #Argus_GetCalibrationPixelRangeOffsets or
- *          #Argus_GetCalibrationGlobalRangeOffset function.
+ *          #Argus_GetCalibrationGlobalRangeOffsets function.
  *
  * @param   hnd The API handle; contains all internal states and data.
  * @param   targetRange The absolute range between the reference plane and the
@@ -1043,28 +1116,40 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd,
  ****************************************************************************/
 
 /*!***************************************************************************
- * @brief   Sets the global range offset value to a specified device.
+ * @brief   Sets the global range offset values to a specified device.
  *
- * @details The global range offset is subtracted from the raw range values.
+ * @details The global range offsets are subtracted from the raw range values.
+ *          There are two distinct values that are applied in low or high
+ *          power stage setting respectively.
  *
  * @param   hnd The API handle; contains all internal states and data.
- * @param   value The new global range offset in meter and Q0.15 format.
+ * @param   offset_low The new global range offset for the low power stage in
+ *                     meter and Q0.15 format.
+ * @param   offset_high The new global range offset for the high power stage in
+ *                      meter and Q0.15 format.
  * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
-		q0_15_t value);
+status_t Argus_SetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
+		q0_15_t offset_low,
+		q0_15_t offset_high);
 
 /*!***************************************************************************
- * @brief   Gets the global range offset value from a specified device.
+ * @brief   Gets the global range offset values from a specified device.
  *
- * @details The global range offset is subtracted from the raw range values.
+ * @details The global range offsets are subtracted from the raw range values.
+ *          There are two distinct values that are applied in low or high
+ *          power stage setting respectively.
  *
  * @param   hnd The API handle; contains all internal states and data.
- * @param   value The current global range offset in meter and Q0.15 format.
+ * @param   offset_low The current range offset for the low power stage in
+ *                     meter and Q0.15 format.
+ * @param   offset_high The current global range offset for the high power stage
+ *                      in meter and Q0.15 format.
  * @return  Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
-		q0_15_t *value);
+status_t Argus_GetCalibrationGlobalRangeOffsets(argus_hnd_t *hnd,
+		q0_15_t *offset_low,
+		q0_15_t *offset_high);
 
 /*!***************************************************************************
  * @brief   Sets the relative pixel offset table to a specified device.
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h
index 8f6b40bdc5b9..8d9a854b2cc1 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h
@@ -210,9 +210,13 @@ typedef enum argus_dca_gain_t {
  *          - [9]: #ARGUS_STATE_LASER_ERROR
  *          - [10]: #ARGUS_STATE_HAS_DATA
  *          - [11]: #ARGUS_STATE_HAS_AUX_DATA
- *          - [12]: #ARGUS_STATE_DCA_MAX
+ *          - [12]: #ARGUS_STATE_SATURATED_PIXELS
  *          - [13]: DCA Power Stage
  *          - [14-15]: DCA Gain Stages
+ *          - [16]: #ARGUS_STATE_DCA_MIN
+ *          - [17]: #ARGUS_STATE_DCA_MAX
+ *          - [18]: #ARGUS_STATE_DCA_RESET
+ *          - [18-31]: not used
  *          .
  *****************************************************************************/
 typedef enum argus_state_t {
@@ -229,36 +233,35 @@ typedef enum argus_state_t {
 	 *          - 1: Enabled: measurement with detuned frequency. */
 	ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U,
 
-	/*! 0x0004: Measurement Frequency for Dual Frequency Mode
+	/*! 0x0004: Measurement Frequency for Dual Frequency Mode \n
 	 *          (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set).
 	 *          - 0: A-Frame w/ detuned frequency,
 	 *          - 1: B-Frame w/ detuned frequency */
 	ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U,
 
-	/*! 0x0008: Debug Mode. If set, the range value of erroneous pixels
+	/*! 0x0008: Debug Mode. \n
+	 *          If set, the range value of erroneous pixels
 	 *          are not cleared or reset.
 	 *          - 0: Disabled (default).
 	 *          - 1: Enabled. */
 	ARGUS_STATE_DEBUG_MODE = 1U << 3U,
 
-	/*! 0x0010: Weak Signal Flag.
+	/*! 0x0010: Weak Signal Flag. \n
 	 *          Set whenever the Pixel Binning Algorithm is detecting a
 	 *          weak signal, i.e. if the amplitude dies not reach its
-	 *          (absolute) threshold. If the Golden Pixel is enabled,
-	 *          this also indicates that the Pixel Binning Algorithm
-	 *          falls back to the Golden Pixel.
+	 *          (absolute) threshold.
 	 *          - 0: Normal Signal.
-	 *          - 1: Weak Signal or Golden Pixel Mode. */
+	 *          - 1: Weak Signal. */
 	ARGUS_STATE_WEAK_SIGNAL = 1U << 4U,
 
-	/*! 0x0020: Background Light Warning Flag.
+	/*! 0x0020: Background Light Warning Flag. \n
 	 *          Set whenever the background light is very high and the
 	 *          measurement data might be unreliable.
 	 *          - 0: No Warning: Background Light is within valid range.
 	 *          - 1: Warning: Background Light is very high. */
 	ARGUS_STATE_BGL_WARNING = 1U << 5U,
 
-	/*! 0x0040: Background Light Error Flag.
+	/*! 0x0040: Background Light Error Flag. \n
 	 *          Set whenever the background light is too high and the
 	 *          measurement data is unreliable or invalid.
 	 *          - 0: No Error: Background Light is within valid range.
@@ -270,7 +273,7 @@ typedef enum argus_state_t {
 	 *          - 1: PLL locked at start of integration. */
 	ARGUS_STATE_PLL_LOCKED = 1U << 7U,
 
-	/*! 0x0100: Laser Failure Warning Flag.
+	/*! 0x0100: Laser Failure Warning Flag. \n
 	 *          Set whenever the an invalid system condition is detected.
 	 *          (i.e. DCA at max state but no amplitude on any (incl. reference)
 	 *          pixel, not amplitude but any saturated pixel).
@@ -279,7 +282,7 @@ typedef enum argus_state_t {
 	 *               condition stays, a laser malfunction error is raised. */
 	ARGUS_STATE_LASER_WARNING = 1U << 8U,
 
-	/*! 0x0200: Laser Failure Error Flag.
+	/*! 0x0200: Laser Failure Error Flag. \n
 	 *          Set whenever a laser malfunction error is raised and the
 	 *          system is put into a safe state.
 	 *          - 0: No Error: Laser is operating properly.
@@ -297,13 +300,12 @@ typedef enum argus_state_t {
 	 *          - 1: Auxiliary data is available and correctly evaluated. */
 	ARGUS_STATE_HAS_AUX_DATA = 1U << 11U,
 
-	/*! 0x1000: DCA Maximum State Flag.
-	 *          Set whenever the DCA has extended all its parameters to their
-	 *          maximum values and can not increase the integration energy any
-	 *          further.
-	 *          - 0: DCA has not yet reached its maximum state.
-	 *          - 1: DCA has reached its maximum state and can not increase any further. */
-	ARGUS_STATE_DCA_MAX = 1U << 12U,
+	/*! 0x0100: Pixel Saturation Flag. \n
+	 *          Set whenever any pixel is saturated, i.e. its pixel state is
+	 *          #PIXEL_SAT
+	 *          - 0: No saturated pixels.
+	 *          - 1: Any saturated pixels. */
+	ARGUS_STATE_SATURATED_PIXELS = 1U << 12U,
 
 	/*! 0x2000: DCA is in high Optical Output Power stage. */
 	ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
@@ -320,6 +322,31 @@ typedef enum argus_state_t {
 	/*! 0xC000: DCA is in high Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT,
 
+	/*! 0x10000: DCA Minimum State Flag. \n
+	 *           Set whenever the DCA has reduced all its parameters to their
+	 *           minimum values and it can not decrease the integration energy
+	 *           any further.
+	 *           - 0: DCA has not yet reached its minimum state.
+	 *           - 1: DCA has reached its minimum state and can not decrease
+	 *                its parameters any further. */
+	ARGUS_STATE_DCA_MIN = 1U << 16U,
+
+	/*! 0x20000: DCA Maximum State Flag. \n
+	 *           Set whenever the DCA has extended all its parameters to their
+	 *           maximum values and it can not increase the integration energy
+	 *           any further.
+	 *           - 0: DCA has not yet reached its maximum state.
+	 *           - 1: DCA has reached its maximum state and can not increase
+	 *                its parameters any further. */
+	ARGUS_STATE_DCA_MAX = 1U << 17U,
+
+	/*! 0x20000: DCA Reset State Flag. \n
+	 *           Set whenever the DCA is resetting all its parameters to their
+	 *           minimum values because it has detected too many saturated pixels.
+	 *           - 0: DCA is operating in normal mode.
+	 *           - 1: DCA is performing a reset. */
+	ARGUS_STATE_DCA_RESET = 1U << 18U,
+
 } argus_state_t;
 
 /*!***************************************************************************
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h
index 4ffa55656ba2..22a85b9d3e24 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h
@@ -58,6 +58,7 @@ extern "C" {
  *****************************************************************************/
 
 #include "utility/int_math.h"
+#include <stdbool.h>
 #include <assert.h>
 
 
@@ -138,6 +139,13 @@ extern "C" {
 #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U))
 
 
+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixels n-index.
+ * @param   n n-index of the pixel.
+ * @return  The pixel mask with only n-index pixel set.
+ ******************************************************************************/
+#define PIXELN_MASK(n) (0x01U << (n))
+
 /*!*****************************************************************************
  * @brief   Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
  * @param   msk 32-bit pixel mask
@@ -151,15 +159,22 @@ extern "C" {
  * @param   msk 32-bit pixel mask
  * @param   n n-index of the pixel to enable.
  ******************************************************************************/
-#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n)))
+#define PIXELN_ENABLE(msk, n) ((msk) |= (PIXELN_MASK(n)))
 
 /*!*****************************************************************************
  * @brief   Macro disable a pixel given by the n-index in a pixel mask.
  * @param   msk 32-bit pixel mask
  * @param   n n-index of the pixel to disable.
  ******************************************************************************/
-#define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n))))
+#define PIXELN_DISABLE(msk, n) ((msk) &= (~PIXELN_MASK(n)))
+
 
+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixels ADC channel number.
+ * @param   c The ADC channel number of the pixel.
+ * @return  The 32-bit pixel mask with only pixel ADC channel set.
+ ******************************************************************************/
+#define PIXELCH_MASK(c) (0x01U << (PIXEL_CH2N(c)))
 
 /*!*****************************************************************************
  * @brief   Macro to determine if an ADC pixel channel is enabled from a pixel mask.
@@ -184,6 +199,14 @@ extern "C" {
 #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c)))
 
 
+/*!*****************************************************************************
+ * @brief   Macro to create a pixel mask given by the pixel x-y-indices.
+ * @param   x x-index of the pixel.
+ * @param   y y-index of the pixel.
+ * @return  The 32-bit pixel mask with only pixel ADC channel set.
+ ******************************************************************************/
+#define PIXELXY_MASK(x, y) (0x01U << (PIXEL_XY2N(x, y)))
+
 /*!*****************************************************************************
  * @brief   Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask.
  * @param   msk 32-bit pixel mask
@@ -337,10 +360,10 @@ static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask,
 
 	uint32_t shifted_mask = 0;
 
-	for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-		for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
-			int8_t x_src = x - dx;
-			int8_t y_src = y - dy;
+	for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
+		for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
+			int8_t x_src = (int8_t)(x - dx);
+			int8_t y_src = (int8_t)(y - dy);
 
 			if (dy & 0x1) {
 				/* Compensate for hexagonal pixel shape. */
@@ -409,8 +432,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
 		int8_t min_y = -1;
 
 		/* Find nearest not selected pixel. */
-		for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
-			for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
+		for (int8_t x = 0; x < ARGUS_PIXELS_X; ++x) {
+			for (int8_t y = 0; y < ARGUS_PIXELS_Y; ++y) {
 				if (!PIXELXY_ISENABLED(pixel_mask, x, y)) {
 					int32_t distx = (x - center_x) << 1;
 
@@ -423,8 +446,8 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
 
 					if (dist < min_dist) {
 						min_dist = dist;
-						min_x = x;
-						min_y = y;
+						min_x = (int8_t)x;
+						min_y = (int8_t)y;
 					}
 				}
 			}
@@ -438,6 +461,64 @@ static inline uint32_t FillPixelMask(uint32_t pixel_mask,
 
 	return pixel_mask;
 }
+
+/*!*****************************************************************************
+ * @brief   Fills a pixel mask with the direct neighboring pixels around a pixel.
+ *
+ * @details The pixel mask is iteratively filled with the direct neighbors of the
+ *          specified center pixel.
+ *
+ *          Note that the function is able to handle corner and edge pixels and
+ *          also to handle odd/even lines (which have different layouts)
+ *
+ * @param   x The selected pixel x-index.
+ * @param   y The selected pixel y-index.
+ * @return  The filled pixel mask with all direct neighbors of the selected pixel.
+ ******************************************************************************/
+static inline uint32_t GetAdjacentPixelsMask(const uint_fast8_t x,
+		const uint_fast8_t y)
+{
+	assert(x < ARGUS_PIXELS_X);
+	assert(y < ARGUS_PIXELS_Y);
+
+	uint32_t mask = 0u;
+
+	bool isXEdgeLow = (x == 0);
+	bool isXEdgeHigh = (x == (ARGUS_PIXELS_X - 1));
+	bool isYEdgeLow = (y == 0);
+	bool isYEdgeHigh = (y == (ARGUS_PIXELS_Y - 1));
+
+	if (y % 2 == 0) {
+		if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x,     y - 1); }
+
+		if ((!isXEdgeHigh) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x + 1, y - 1); }
+
+		if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
+
+		if ((!isXEdgeHigh) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x + 1, y + 1); }
+
+		if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x,     y + 1); }
+
+		if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
+
+	} else {
+		if ((!isXEdgeLow) && (!isYEdgeLow)) { PIXELXY_ENABLE(mask, x - 1, y - 1); }
+
+		if (!isYEdgeLow) { PIXELXY_ENABLE(mask, x,     y - 1); }
+
+		if (!isXEdgeHigh) { PIXELXY_ENABLE(mask, x + 1, y); }
+
+		if (!isYEdgeHigh) { PIXELXY_ENABLE(mask, x,     y + 1); }
+
+		if ((!isXEdgeLow) && (!isYEdgeHigh)) { PIXELXY_ENABLE(mask, x - 1, y + 1); }
+
+		if (!isXEdgeLow) { PIXELXY_ENABLE(mask, x - 1, y); }
+	}
+
+	return mask;
+}
+
+
 /*! @} */
 #ifdef __cplusplus
 } // extern "C"
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h
deleted file mode 100644
index 258fb3826092..000000000000
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_msk.h
+++ /dev/null
@@ -1,170 +0,0 @@
-/*************************************************************************//**
- * @file
- * @brief    	This file is part of the AFBR-S50 API.
- * @details		Defines macros to work with pixel and ADC channel masks.
- *
- * @copyright
- *
- * Copyright (c) 2021, Broadcom Inc
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * 1. Redistributions of source code must retain the above copyright notice, this
- *    list of conditions and the following disclaimer.
- *
- * 2. Redistributions in binary form must reproduce the above copyright notice,
- *    this list of conditions and the following disclaimer in the documentation
- *    and/or other materials provided with the distribution.
- *
- * 3. Neither the name of the copyright holder nor the names of its
- *    contributors may be used to endorse or promote products derived from
- *    this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
- * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
- * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
- * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
- * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *****************************************************************************/
-
-#ifndef ARGUS_MSK_H
-#define ARGUS_MSK_H
-
-/*!***************************************************************************
- * @defgroup	argusmap ADC Channel Mapping
- * @ingroup		argusres
- *
- * @brief		Pixel ADC Channel (n) to x-y-Index Mapping
- *
- * @details		The ADC Channels of each pixel or auxiliary channel on the device
- * 				is numbered in a way that is convenient on the chip. The macros
- * 				in this module are defined in order to obtain the x-y-indices of
- * 				each channel and vice versa.
- *
- * @addtogroup 	argusmap
- * @{
- *****************************************************************************/
-
-#include "api/argus_def.h"
-#include "utility/int_math.h"
-
-/*!*****************************************************************************
- * @brief	Macro to determine the channel number of an specified Pixel.
- * @param	x The x index of the pixel.
- * @param	y The y index of the pixel.
- * @return	The channel number n of the pixel.
- ******************************************************************************/
-#define PIXEL_XY2N(x, y) ((((x) ^ 7) << 1) | ((y) & 2) << 3 | ((y) & 1))
-
-/*!*****************************************************************************
- * @brief	Macro to determine the x index of an specified Pixel channel.
- * @param	n The channel number of the pixel.
- * @return	The x index number of the pixel.
- ******************************************************************************/
-#define PIXEL_N2X(n) ((((n) >> 1U) & 7) ^ 7)
-
-/*!*****************************************************************************
- * @brief	Macro to determine the y index of an specified Pixel channel.
- * @param	n The channel number of the pixel.
- * @return	The y index number of the pixel.
- ******************************************************************************/
-#define PIXEL_N2Y(n) (((n) & 1U) | (((n) >> 3) & 2U))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC Pixel channel was enabled from a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- * @return 	True if the pixel channel n was enabled, false elsewise.
- ******************************************************************************/
-#define PIXELN_ISENABLED(msk, ch) (((msk) >> (ch)) & 0x01U)
-
-/*!*****************************************************************************
- * @brief	Macro enables an ADC Pixel channel in a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- ******************************************************************************/
-#define PIXELN_ENABLE(msk, ch) ((msk) |= (0x01U << (ch)))
-
-/*!*****************************************************************************
- * @brief	Macro disables an ADC Pixel channel in a pixel mask.
- * @param	msk The 32-bit pixel mask
- * @param	ch The channel number of the pixel.
- ******************************************************************************/
-#define PIXELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << (ch))))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if an ADC Pixel channel was enabled from a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- * @return 	True if the pixel (x,y) was enabled, false elsewise.
- ******************************************************************************/
-#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro enables an ADC Pixel channel in a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- ******************************************************************************/
-#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro disables an ADC Pixel channel in a pixel mask.
- * @param	msk 32-bit pixel mask
- * @param	x x index of the pixel.
- * @param	y y index of the pixel.
- ******************************************************************************/
-#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y)))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U)
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U)))
-
-/*!*****************************************************************************
- * @brief	Macro to determine if a ADC channel was enabled from a channel mask.
- * @param	msk 32-bit channel mask
- * @param	ch channel number of the ADC channel.
- * @return 	True if the ADC channel n was enabled, false elsewise.
- ******************************************************************************/
-#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U))))
-
-
-/*!*****************************************************************************
- * @brief	Macro to determine the number of enabled pixel channels via a popcount
- * 			algorithm.
- * @param	pxmsk 32-bit pixel mask
- * @return 	The count of enabled pixel channels.
- ******************************************************************************/
-#define PIXEL_COUNT(pxmsk) popcount(pxmsk)
-
-/*!*****************************************************************************
- * @brief	Macro to determine the number of enabled channels via a popcount
- * 			algorithm.
- * @param	pxmsk 32-bit pixel mask
- * @param	chmsk 32-bit channel mask
- * @return 	The count of enabled ADC channels.
- ******************************************************************************/
-#define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk))
-
-/*! @} */
-#endif /* ARGUS_MSK_H */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h
index 7a41440f3976..3ef649d45b3b 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h
@@ -36,6 +36,9 @@
 
 #ifndef ARGUS_OFFSET_H
 #define ARGUS_OFFSET_H
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 /*!***************************************************************************
  * @addtogroup  argus_cal
@@ -48,12 +51,26 @@
  * @brief   Pixel Range Offset Table.
  * @details Contains pixel range offset values for all 32 active pixels.
  *****************************************************************************/
-typedef struct argus_cal_offset_table_t {
-	/*! The offset values per pixel in meter and Q0.15 format. */
-	q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+typedef union argus_cal_offset_table_t {
+	struct {
+		/*! The offset values table for Low Power Stage of all 32 pixels.
+		 *  Unit: meter; Format: Q0.15 */
+		q0_15_t LowPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+
+		/*! The offset values table for High Power Stage of all 32 pixels.
+		 *  Unit: meter; Format: Q0.15 */
+		q0_15_t HighPower[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+	};
+
+	/*! The offset values table for Low/High Power Stages of all 32 pixels.
+	 *  Unit: meter; Format: Q0.15 */
+	q0_15_t Table[ARGUS_DCA_POWER_STAGE_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
 
 } argus_cal_offset_table_t;
 
 
 /*! @} */
+#ifdef __cplusplus
+} // extern "C"
+#endif
 #endif /* ARGUS_OFFSET_T */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h
index f28576500d11..f412229396a6 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h
@@ -55,11 +55,11 @@ extern "C" {
  *              information from the filtered pixels by averaging them in a
  *              specified way.
  *
- *              The Pixel Binning Algorithm is a three-stage filter with a
- *              fallback value:
+ *              Basically, the Pixel Binning Algorithm is a multi-stage filter:
  *
  *              -# A fixed pre-filter mask is applied to statically disable
  *                  specified pixels.
+ *
  *              -# A relative and absolute amplitude filter is applied in the
  *                  second stage. The relative filter is determined by a ratio
  *                  of the maximum amplitude off all available (i.e. not filtered
@@ -75,12 +75,28 @@ extern "C" {
  *                  selected and considered for the final 1D distance. The
  *                  absolute threshold is used to dismiss pixels that are below
  *                  the noise level. The latter would be considered for the 1D
- *                  result if the maximum amplitude is already very low.
+ *                  result if the maximum amplitude is already very low.\n
+ *                  Those threshold are implemented using a hysteresis behavior.
+ *                  For its configuration, see the following parameters:
+ *                  - #argus_cfg_pba_t::RelativeAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::RelativeAmplitudeExclusion
+ *                  - #argus_cfg_pba_t::AbsoluteAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::AbsoluteAmplitudeExclusion
+ *                  .
+ *
  *              -# An absolute minimum distance filter is applied in addition
  *                  to the amplitude filter. This removes all pixel that have
  *                  a lower distance than the specified threshold. This is used
  *                  to remove invalid pixels that can be detected by a physically
- *                  not correct negative distance.
+ *                  not correct negative distance.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_MIN_DIST_SCOPE
+ *                  - #argus_cfg_pba_t::AbsoluteDistanceScopeInclusion
+ *                  - #argus_cfg_pba_t::AbsoluteDistanceScopeExclusion
+ *                  - #argus_cfg_pba_t::RelativeDistanceScopeInclusion
+ *                  - #argus_cfg_pba_t::RelativeDistanceScopeExclusion
+ *                  .
+ *
  *              -# A distance filter is used to distinguish pixels that target
  *                  the actual object from pixels that see the brighter background,
  *                  e.g. white walls. Thus, the pixel with the minimum distance
@@ -90,11 +106,31 @@ extern "C" {
  *                  determined by an relative (to the current minimum distance)
  *                  and an absolute value. The larger scope value is the
  *                  relevant one, i.e. the relative distance scope can be used
- *                  to heed the increasing noise at larger distances.
+ *                  to heed the increasing noise at larger distances.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #argus_cfg_pba_t::AbsoluteMinimumDistanceThreshold
+ *                  .
+ *
  *              -# If all of the above filters fail to determine a single valid
  *                  pixel, the Golden Pixel is used as a fallback value. The
  *                  Golden Pixel is the pixel that sits right at the focus point
- *                  of the optics at large distances.
+ *                  of the optics at large distances. Thus, it is expected to
+ *                  have the best signal at large distances.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
+ *                  .
+ *
+ *              -# In order to avoid unwanted effects from "out-of-focus" pixels
+ *                  in application that require a smaller focus, the Golden Pixel
+ *                  Priority Mode prioritizes a valid signal on the central
+ *                  Golden Pixel over other pixels. That is, while the Golden
+ *                  Pixel has a reasonable signal strength, it is the only pixel
+ *                  considered for the 1D result.\n
+ *                  For its configuration, see the following parameters:
+ *                  - #PBA_ENABLE_GOLDPX_FALLBACK_MODE
+ *                  - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeInclusion
+ *                  - #argus_cfg_pba_t::GoldenPixelPriorityAmplitudeExclusion
+ *                  .
  *              .
  *
  *              After filtering is done, there may be more than a single pixel
@@ -113,14 +149,17 @@ extern "C" {
  * @brief   Enable flags for the pixel binning algorithm.
  *
  * @details Determines the pixel binning algorithm feature enable status.
+ *
  *          - [0]: #PBA_ENABLE: Enables the pixel binning feature.
  *          - [1]: reserved
  *          - [2]: reserved
  *          - [3]: reserved
- *          - [4]: reserved
- *          - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature.
- *          - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope
- *                   feature.
+ *          - [4]: #PBA_ENABLE_GOLDPX_PRIORITY_MODE: Enables the Golden Pixel
+ *                                                   priority mode feature.
+ *          - [5]: #PBA_ENABLE_GOLDPX_FALLBACK_MODE: Enables the Golden Pixel
+ *                                                   fallback mode feature.
+ *          - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance
+ *                                             scope feature.
  *          - [7]: reserved
  *          .
  *****************************************************************************/
@@ -128,8 +167,17 @@ typedef enum argus_pba_flags_t {
 	/*! Enables the pixel binning feature. */
 	PBA_ENABLE = 1U << 0U,
 
-	/*! Enables the Golden Pixel. */
-	PBA_ENABLE_GOLDPX = 1U << 5U,
+	/*! Enables the Golden Pixel Priority Mode.
+	 *  If enabled, the Golden Pixel is prioritized over other Pixels as long
+	 *  as it has a good signal (determined by # */
+	PBA_ENABLE_GOLDPX_PRIORITY_MODE = 1U << 4U,
+
+	/*! Enables the Golden Pixel Fallback Mode.
+	 *  If enabled, the Golden Pixel is used as a last fallback pixel to obtain
+	 *  a valid signal from. This is recommended for all non-multi pixel
+	 *  devices whose TX field-of-view is aligned to target the Golden Pixel in
+	 *  factory calibration. */
+	PBA_ENABLE_GOLDPX_FALLBACK_MODE = 1U << 5U,
 
 	/*! Enables the minimum distance scope filter. */
 	PBA_ENABLE_MIN_DIST_SCOPE = 1U << 6U,
@@ -168,65 +216,297 @@ typedef struct {
 	 *  about the individual evaluation modes. */
 	argus_pba_averaging_mode_t AveragingMode;
 
-	/*! The Relative amplitude threshold value (in %) of the max. amplitude.
+	/*! The relative amplitude inclusion threshold (in %) of the max. amplitude.
 	 *
-	 *  Pixels with amplitude below this threshold value are dismissed.
+	 *  Pixels, whose amplitudes raise above this inclusion threshold, are
+	 *  added to the pixel binning. The amplitude must fall below the
+	 *  exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
+	 *  the pixel binning again.
 	 *
 	 *  All available values from the 8-bit representation are valid.
 	 *  The actual percentage value is determined by 100%/256*x.
 	 *
-	 *  Use 0 to disable the relative amplitude threshold. */
-	uq0_8_t RelAmplThreshold;
+	 *  Note: in addition to the relative criteria, there is also the absolute
+	 *  criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be greater than or equal to the #RelativeAmplitudeExclusion.
+	 *
+	 *  Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #RelativeAmplitudeExclusion and
+	 *  #RelativeAmplitudeInclusion) to disable the relative amplitude
+	 *  hysteresis. */
+	uq0_8_t RelativeAmplitudeInclusion;
 
-	/*! The relative minimum distance scope value in %.
+	/*! The relative amplitude exclusion threshold (in %) of the max. amplitude.
 	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 *  Pixels, whose amplitudes fall below this exclusion threshold, are
+	 *  removed from the pixel binning. The amplitude must raise above the
+	 *  inclusion (#RelativeAmplitudeInclusion) threshold to be added back
+	 *  to be pixel binning again.
 	 *
 	 *  All available values from the 8-bit representation are valid.
 	 *  The actual percentage value is determined by 100%/256*x.
 	 *
-	 *  Special values:
-	 *  - 0: Use 0 for absolute value only or to choose the pixel with the
-	 *       minimum distance only (of also the absolute value is 0)! */
-	uq0_8_t RelMinDistanceScope;
+	 *  Note: in addition to the relative criteria, there is also the absolute
+	 *  criteria (#AbsoluteAmplitudeInclusion, #AbsoluteAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be less than or equal to #RelativeAmplitudeInclusion.
+	 *
+	 *  Use #RelativeAmplitudeExclusion == #RelativeAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #RelativeAmplitudeExclusion and
+	 *  #RelativeAmplitudeInclusion) to disable the relative amplitude
+	 *  hysteresis. */
+	uq0_8_t RelativeAmplitudeExclusion;
+
+	/*! The absolute amplitude inclusion threshold in LSB.
+	 *
+	 *  Pixels, whose amplitudes raise above this inclusion threshold, are
+	 *  added to the pixel binning. The amplitude must fall below the
+	 *  exclusion (#RelativeAmplitudeExclusion) threshold to be removed from
+	 *  the pixel binning again.
+	 *
+	 *  The absolute amplitude hysteresis is only valid if the Golden Pixel
+	 *  mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
+	 *  which disables the absolute criteria.
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Note: in addition to the absolute criteria, there is also the relative
+	 *  criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be greater than or equal to #AbsoluteAmplitudeExclusion.
+	 *
+	 *  Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #AbsoluteAmplitudeExclusion and
+	 *  #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
+	 *  hysteresis. */
+	uq12_4_t AbsoluteAmplitudeInclusion;
+
+	/*! The absolute amplitude exclusion threshold in LSB.
+	 *
+	 *  Pixels, whose amplitudes fall below this exclusion threshold, are
+	 *  removed from the pixel binning. The amplitude must raise above the
+	 *  inclusion (#RelativeAmplitudeInclusion) threshold to be added back
+	 *  to be pixel binning again.
+	 *
+	 *  The absolute amplitude hysteresis is only valid if the Golden Pixel
+	 *  mode is enabled. Otherwise, the thresholds are set to 0 LSB internally
+	 *  which disables the absolute criteria.
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Note: in addition to the absolute criteria, there is also the relative
+	 *  criteria (#RelativeAmplitudeInclusion, #RelativeAmplitudeExclusion).
+	 *  The pixels are added to the pixel binning if their respective amplitude
+	 *  is larger than the absolute AND relative inclusion values. On the other
+	 *  hand, they are removed if their amplitude falls below the absolute OR
+	 *  relative exclusion threshold.
+	 *
+	 *  Must be less than or equal to #AbsoluteAmplitudeInclusion.
+	 *
+	 *  Use #AbsoluteAmplitudeExclusion == #AbsoluteAmplitudeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only.
+	 *
+	 *  Use 0 (for both, #AbsoluteAmplitudeExclusion and
+	 *  #AbsoluteAmplitudeInclusion) to disable the absolute amplitude
+	 *  hysteresis. */
+	uq12_4_t AbsoluteAmplitudeExclusion;
+
+	/*! The Golden Pixel Priority Mode inclusion threshold in LSB.
+	 *
+	 *  The Golden Pixel Priority Mode prioritizes a valid signal on the
+	 *  Golden Pixel over other pixel to avoid unwanted effects from
+	 *  "out-of-focus" pixels in application that require a smaller focus.
+	 *
+	 *  If the Golden Pixel priority mode is enabled (see
+	 *  #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid signal
+	 *  with amplitude higher than this inclusion threshold, its priority state
+	 *  is enabled and the binning exits early by dismissing all other pixels
+	 *  regardless of their respective amplitude or state. The Golden Pixel
+	 *  priority state is disabled if the Golden Pixel amplitude falls below
+	 *  the exclusion threshold (#GoldenPixelPriorityAmplitudeExclusion) or its
+	 *  state becomes invalid (e.g. #PIXEL_SAT).
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/16.
+	 *
+	 *  Use 0 to disable the Golden Pixel priority mode hysteresis. */
+	uq12_4_t GoldenPixelPriorityAmplitudeInclusion;
 
-	/*! The absolute amplitude threshold value in LSB.
+	/*! The Golden Pixel Priority Mode exclusion threshold in LSB.
 	 *
-	 *  Pixels with amplitude below this threshold value are dismissed.
+	 *  The Golden Pixel Priority Mode prioritizes a valid signal on the
+	 *  Golden Pixel over other pixel to avoid unwanted effects from
+	 *  "out-of-focus" pixels in application that require a smaller focus.
 	 *
-	 *  The absolute amplitude threshold is only valid if the Golden Pixel
-	 *  mode is enabled. Otherwise, the threshold is set to 0 LSB internally.
+	 *  If the Golden Pixel priority mode is enabled (see
+	 *  #PBA_ENABLE_GOLDPX_PRIORITY_MODE) and the Golden Pixel has a valid
+	 *  signal with amplitude higher than the exclusion threshold
+	 *  (#GoldenPixelPriorityAmplitudeInclusion), its priority state is enabled
+	 *  and the binning exits early by dismissing all other pixels regardless
+	 *  of their respective amplitude or state. The Golden Pixel priority state
+	 *  is disabled if the Golden Pixel amplitude falls below this exclusion
+	 *  threshold or its state becomes invalid (e.g. #PIXEL_SAT).
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/16.
 	 *
-	 *  Use 0 to disable the absolute amplitude threshold. */
-	uq12_4_t AbsAmplThreshold;
+	 *  Use 0 to disable the Golden Pixel priority mode hysteresis. */
+	uq12_4_t GoldenPixelPriorityAmplitudeExclusion;
+
+	/*! The relative minimum distance scope inclusion threshold (in %).
+	 *
+	 *  Pixels, whose range is smaller than the minimum distance inclusion
+	 *  threshold (x_min + dx_incl) are added to the pixel binning. The
+	 *  range must raise above the exclusion
+	 *  (#RelativeDistanceScopeExclusion) threshold to be removed
+	 *  from the pixel binning again. The relative value is determined
+	 *  by multiplying the percentage with the minimum distance.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute inclusion values
+	 *  (#AbsoluteDistanceScopeInclusion).
+	 *
+	 *  All available values from the 8-bit representation are valid.
+	 *  The actual percentage value is determined by 100%/256*x.
+	 *
+	 *  Must be smaller than or equal to the #RelativeDistanceScopeExclusion.
+	 *
+	 *  Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq0_8_t RelativeDistanceScopeInclusion;
+
+	/*! The relative distance scope exclusion threshold (in %).
+	 *
+	 *  Pixels, whose range is larger than the minimum distance exclusion
+	 *  threshold (x_min + dx_excl) are removed from the pixel binning. The
+	 *  range must fall below the inclusion
+	 *  (#RelativeDistanceScopeInclusion) threshold to be added
+	 *  to the pixel binning again. The relative value is determined
+	 *  by multiplying the percentage with the minimum distance.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#AbsoluteDistanceScopeExclusion).
+	 *
+	 *  All available values from the 8-bit representation are valid.
+	 *  The actual percentage value is determined by 100%/256*x.
+	 *
+	 *  Must be larger than or equal to the #RelativeDistanceScopeInclusion.
+	 *
+	 *  Use #RelativeDistanceScopeExclusion == #RelativeDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq0_8_t RelativeDistanceScopeExclusion;
 
-	/*! The absolute minimum distance scope value in m.
+	/*! The absolute minimum distance scope inclusion threshold (in m).
 	 *
-	 *  Pixels that have a range value within [x0, x0 + dx] are considered
-	 *  for the pixel binning, where x0 is the minimum distance of all
-	 *  amplitude picked pixels and dx is the minimum distance scope value.
-	 *  The minimum distance scope value will be the maximum of relative
-	 *  and absolute value.
+	 *  Pixels, whose range is smaller than the minimum distance inclusion
+	 *  threshold (x_min + dx_incl) are added to the pixel binning. The
+	 *  range must raise above the exclusion
+	 *  (#AbsoluteDistanceScopeExclusion) threshold to be added
+	 *  to the pixel binning again.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#RelativeDistanceScopeInclusion).
+	 *
+	 *  All available values from the 16-bit representation are valid.
+	 *  The actual LSB value is determined by x/2^15.
+	 *
+	 *  Must be smaller than or equal to the #AbsoluteDistanceScopeExclusion.
+	 *
+	 *  Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq1_15_t AbsoluteDistanceScopeInclusion;
+
+	/*! The absolute minimum distance scope exclusion threshold (in m).
+	 *
+	 *  Pixels, whose range is larger than the minimum distance exclusion
+	 *  threshold (x_min + dx_excl) are removed from the pixel binning. The
+	 *  range must fall below the inclusion
+	 *  (#AbsoluteDistanceScopeInclusion) threshold to be added
+	 *  to the pixel binning again.
+	 *
+	 *  The distance scope determines an interval within that pixels
+	 *  are considered valid, originating at the minimum distance (x_min).
+	 *  The width of the interval is specified by the relative and absolute
+	 *  minimum distance scope thresholds. The actual values it the
+	 *  maximum of both, the relative and absolute exclusion values
+	 *  (#RelativeDistanceScopeExclusion).
 	 *
 	 *  All available values from the 16-bit representation are valid.
 	 *  The actual LSB value is determined by x/2^15.
 	 *
-	 *  Special values:
-	 *  - 0: Use 0 for relative value only or to choose the pixel with the
-	 *       minimum distance only (of also the relative value is 0)! */
-	uq1_15_t AbsMinDistanceScope;
+	 *  Must be larger than or equal to the #AbsoluteDistanceScopeInclusion.
+	 *
+	 *  Use #AbsoluteDistanceScopeExclusion == #AbsoluteDistanceScopeInclusion to
+	 *  disable the hysteresis behavior and use it as a threshold only. */
+	uq1_15_t AbsoluteDistanceScopeExclusion;
+
+	/*! The Golden Pixel Saturation Filter Pixel Threshold.
+	 *
+	 *  The Golden Pixel Saturation Filter will evaluate the status of the
+	 *  Golden Pixel to #PIXEL_INVALID if a certain number of active pixels,
+	 *  i.e. pixels that are not removed by the static pre-filter mask
+	 *  (#PrefilterMask), are saturated (#PIXEL_SAT).
+	 *
+	 *  The purpose of this filter is to avoid erroneous situations with highly
+	 *  reflective targets (e.g. retro-reflectors) that can invalidate the
+	 *  Golden Pixel such that it would not show the correct saturation state.
+	 *  In order to avoid using the Golden Pixel in that scenario, this filter
+	 *  mechanism can be used to remove the Golden Pixel if a specified number
+	 *  of other pixels show saturation state.
+	 *
+	 *  Use 0 to disable the Golden Pixel Saturation Filter. */
+	uint8_t GoldenPixelSaturationFilterPixelThreshold;
+
+	/*! The Golden Pixel out-of-sync age limit for the GPPM.
+	 *
+	 *  The Golden Pixel out-of-sync age is the number of consecutive frames
+	 *  where the Golden Pixel is out-of-sync. This parameters is the threshold
+	 *  to distinguish between temporary and permanent out-of-sync states.
+	 *
+	 *  Temporary out-of-sync states happen when the target rapidly changes. In
+	 *  this case, the Golden Pixel Priority Mode (GPPM) is not exited. Only if
+	 *  the out-of-sync age exceeds the specified threshold, the Golden Pixel is
+	 *  considered erroneous and the GPPM is exited.
+	 *
+	 *  Use 0 to disable the Golden Pixel out-of-sync aging (= infinity). */
+	uint8_t GoldenPixelOutOfSyncAgeThreshold;
 
 	/*! The absolute minimum distance threshold value in m.
 	 *
 	 *  Pixels with distance below this threshold value are dismissed. */
-	q9_22_t AbsMinDistanceThreshold;
+	q9_22_t AbsoluteMinimumDistanceThreshold;
 
 	/*! The pre-filter pixel mask determines the pixel channels that are
 	 *  statically excluded from the pixel binning (i.e. 1D distance) result.
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h
index a739cea7f367..3d4ef3d50ce4 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h
@@ -55,6 +55,9 @@ extern "C" {
  * Also used as a special value to determine no object detected or infinity range. */
 #define ARGUS_RANGE_MAX (Q9_22_MAX)
 
+/*! Minimum range value in Q9.22 format. */
+#define ARGUS_RANGE_MIN (Q9_22_MIN)
+
 /*!***************************************************************************
  * @brief   Status flags for the evaluated pixel structure.
  *
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h
index 77cd85641363..8f3fb09688f4 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h
@@ -227,12 +227,19 @@ enum Status {
 
 	/*! -114: AFBR-S50 Error: Register data integrity is lost (e.g. due to unexpected
 	 *  power-on-reset cycle or invalid write cycle of SPI. System tries to
-	 *  reset the values. */
+	 *  reset the values.
+	 *
+	 *  @note If this error occurs after intentionally cycling the power supply
+	 *  of the device, use the #Argus_RestoreDeviceState API function to properly
+	 *  recover the current API state into the device to avoid that issue. */
 	ERROR_ARGUS_DATA_INTEGRITY_LOST = -114,
 
 	/*! -115: AFBR-S50 Error: The range offsets calibration failed! */
 	ERROR_ARGUS_RANGE_OFFSET_CALIBRATION_FAILED = -115,
 
+	/*! -116: AFBR-S50 Error: The VSUB calibration failed! */
+	ERROR_ARGUS_VSUB_CALIBRATION_FAILED = -116,
+
 	/*! -191: AFBR-S50 Error: The device is currently busy and cannot execute the
 	 *  requested command. */
 	ERROR_ARGUS_BUSY = -191,
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h
index a1a2d878ac99..f58ba1bba74e 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h
@@ -56,13 +56,13 @@ extern "C" {
 #define ARGUS_API_VERSION_MAJOR    1
 
 /*! Minor version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_MINOR    4
+#define ARGUS_API_VERSION_MINOR    5
 
 /*! Bugfix version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUGFIX   4
+#define ARGUS_API_VERSION_BUGFIX   6
 
 /*! Build version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUILD    "20230327150535"
+#define ARGUS_API_VERSION_BUILD    "20240208081753"
 
 /*****************************************************************************/
 
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h
index 6f3d40b49a42..284538a52bc9 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h
@@ -72,30 +72,28 @@ typedef struct xtalk_t {
  * @details Contains crosstalk vector values for all 32 active pixels,
  *          separated for A/B-Frames.
  *****************************************************************************/
-typedef struct argus_cal_xtalk_table_t {
-	union {
-		struct {
-			/*! The crosstalk vector table for A-Frames. */
-			xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-
-			/*! The crosstalk vector table for B-Frames. */
-			xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
-		};
-
-		/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
-		xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+typedef union argus_cal_xtalk_table_t {
+	struct {
+		/*! The crosstalk vector table for A-Frames. */
+		xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+
+		/*! The crosstalk vector table for B-Frames. */
+		xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
 	};
 
+	/*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/
+	xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+
 } argus_cal_xtalk_table_t;
 
 
 /*!***************************************************************************
- * @brief   Pixel-To-Pixel Crosstalk Compensation Parameters.
- * @details Contains calibration data that belongs to the pixel-to-pixel
- *          crosstalk compensation feature.
+ * @brief   Electrical Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains calibration data that belongs to the electrical
+ *          pixel-to-pixel crosstalk compensation feature.
  *****************************************************************************/
-typedef struct argus_cal_p2pxtalk_t {
-	/*! Pixel-To-Pixel Compensation on/off. */
+typedef struct argus_cal_electrical_p2pxtalk_t {
+	/*! Electrical Pixel-To-Pixel Compensation on/off. */
 	bool Enabled;
 
 	/*! The relative threshold determines when the compensation is active for
@@ -134,8 +132,39 @@ typedef struct argus_cal_p2pxtalk_t {
 	 *  Higher values determine more influence on the reference pixel signal. */
 	q3_12_t KcFactorCRefPx;
 
-} argus_cal_p2pxtalk_t;
+} argus_cal_electrical_p2pxtalk_t;
+
+/*!***************************************************************************
+ * @brief   Optical Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains calibration data that belongs to the optical
+ *          pixel-to-pixel crosstalk compensation feature.
+ *****************************************************************************/
+typedef struct argus_cal_optical_p2pxtalk_t {
+	/*! Optical Pixel-To-Pixel Compensation on/off. */
+	bool Enabled;
+
+	/*! The sine component of the coupling coefficient that determines the amount
+	 *  of a neighbour pixel signal that influences the raw signal of certain pixel.
+	 *  Higher values determine more influence on the individual pixel signal. */
+	q3_12_t CouplingCoeffS;
+
+	/*! The cosine component of the coupling coefficient that determines the amount
+	 *  of a neighbour pixel signal that influences the raw signal of a certain pixel.
+	 *  Higher values determine more influence on the individual pixel signal. */
+	q3_12_t CouplingCoeffC;
+
+} argus_cal_optical_p2pxtalk_t;
+
+/*!***************************************************************************
+ * @brief   Pixel-To-Pixel Crosstalk Compensation Parameters.
+ * @details Contains combined calibration data for electrical and
+ *          optical pixel-to-pixel crosstalk compensation feature.
+ *****************************************************************************/
+typedef struct argus_cal_p2pxtalk_t {
+	argus_cal_electrical_p2pxtalk_t Electrical;
 
+	argus_cal_optical_p2pxtalk_t Optical;
+} argus_cal_p2pxtalk_t;
 
 /*! @} */
 #ifdef __cplusplus
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h
index 60b75a164a3e..09c6fdfdc3d7 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h
@@ -61,7 +61,7 @@ extern "C" {
  * @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit
  *          architecture with maximum precision.
  *          The result is correctly rounded and given as the input format.
- *          Division by 0 yields max. values determined by signa of numerator.
+ *          Division by 0 yields max. values determined by signs of numerator.
  *          Too high/low results are truncated to max/min values.
  *
  *          Depending on the architecture, the division is implemented with a 64-bit
@@ -89,14 +89,14 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)
 
 		if (c > 0x80000000U) { return INT32_MIN; }
 
-		return -c;
+		return (int32_t) - c;
 
 	} else {
 		c = ((c / b) + (1 << 13U)) >> 14U;
 
 		if (c > (int64_t)INT32_MAX) { return INT32_MAX; }
 
-		return c;
+		return (int32_t)c;
 	}
 
 #else
@@ -159,10 +159,16 @@ inline int32_t fp_div16(int32_t a, q15_16_t b)
 
 	/* Figure out the sign of result */
 	if ((uint32_t)(a ^ b) & 0x80000000U) {
-		result = -result;
+		return (int32_t) - result;
+
+	} else {
+		// fix 05.10.2023; the corner case, when result == INT32_MAX + 1:
+		// Catch the wraparound (to INT32_MIN) and truncate instead.
+		if (quotient > INT32_MAX) { return INT32_MAX; }
+
+		return (int32_t)result;
 	}
 
-	return (int32_t)result;
 #endif
 }
 
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h
index 78db5826445a..e0996d6d78a5 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h
@@ -118,7 +118,7 @@ inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift)
 	assert(shift <= 32);
 #if USE_64BIT_MUL
 	const uint64_t w = (uint64_t)u * (uint64_t)v;
-	return (w >> shift) + ((w >> (shift - 1)) & 1U);
+	return (uint32_t)((w >> shift) + ((w >> (shift - 1)) & 1U));
 #else
 	uint32_t tmp[2] = { 0 };
 	muldwu(tmp, u, v);
@@ -158,15 +158,15 @@ inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift)
 
 	uint32_t u2, v2;
 
-	if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; }
+	if (u < 0) { u2 = (uint32_t) - u; sign = -sign; } else { u2 = (uint32_t)u; }
 
-	if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; }
+	if (v < 0) { v2 = (uint32_t) - v; sign = -sign; } else { v2 = (uint32_t)v; }
 
 	const uint32_t res = fp_mulu(u2, v2, shift);
 
 	assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U);
 
-	return sign > 0 ? res : -res;
+	return sign > 0 ? (int32_t)res : -(int32_t)res;
 }
 
 
@@ -225,7 +225,9 @@ inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift)
  *****************************************************************************/
 inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift)
 {
-	return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift);
+	return u >= 0 ?
+	       (int32_t)fp_mul_u32_u16((uint32_t)u, v, shift) :
+	       -(int32_t)fp_mul_u32_u16((uint32_t) - u, v, shift);
 }
 
 /*! @} */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h
index ad6f71e09cc6..056f2e027df2 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h
@@ -80,7 +80,7 @@ inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n)
  *****************************************************************************/
 inline int32_t fp_rnds(int32_t Q, uint_fast8_t n)
 {
-	return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n);
+	return (Q < 0) ? -(int32_t)fp_rndu((uint32_t)(-Q), n) : (int32_t)fp_rndu((uint32_t)Q, n);
 }
 
 /*!***************************************************************************
@@ -108,7 +108,7 @@ inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n)
  *****************************************************************************/
 inline int32_t fp_truncs(int32_t Q, uint_fast8_t n)
 {
-	return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n);
+	return (Q < 0) ? -(int32_t)fp_truncu((uint32_t)(-Q), n) : (int32_t)fp_truncu((uint32_t)Q, n);
 }
 
 /*! @} */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h
index 27de8cc68897..b9b200414663 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h
@@ -66,7 +66,7 @@ inline uint32_t log2i(uint32_t x)
 {
 	assert(x != 0);
 #if 1
-	return 31 - __builtin_clz(x);
+	return (uint32_t)(31 - __builtin_clz(x));
 #else
 #define S(k) if (x >= (1 << k)) { i += k; x >>= k; }
 	int i = 0; S(16); S(8); S(4); S(2); S(1); return i;
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a
index e4e6dc0c52cd..1cd0fe27e0df 100644
Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a differ
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a
index 23bcd6fbea11..a6e7c581d6d4 100644
Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a differ