diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
index 22c3105c890..af1d5a1eb5a 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
@@ -94,17 +94,16 @@ void AFBRS50::ProcessMeasurement(void *data)
 		argus_results_t res{};
 		status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data);
 
-		if ((evaluate_status == STATUS_OK) && (res.Status == 0)) {
+		if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) {
 			uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
 			float result_m = static_cast<float>(result_mm) / 1000.f;
-			//int8_t quality = res.Bin.SignalQuality;
-			int8_t quality = 100;
+			int8_t quality = res.Bin.SignalQuality;
 
 			// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
 			// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
-			//if (quality == 1) {
-			//	quality = 0;
-			//}
+			if (quality == 1) {
+				quality = 0;
+			}
 
 			// distance quality check
 			if (result_m > _max_distance) {
@@ -264,7 +263,6 @@ void AFBRS50::Run()
 
 			if (status != STATUS_OK) {
 				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
-				_state = STATE::STOP;
 				ScheduleNow();
 
 			} else {
@@ -316,6 +314,12 @@ void AFBRS50::UpdateMode()
 				PX4_ERR("set_rate status not okay: %i", (int)status);
 			}
 
+			status = set_rate(LONG_RANGE_MODE_HZ);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("set_rate status not okay: %i", (int)status);
+			}
+
 		} else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) {
 			// change to short range mode
 			argus_mode_t mode = ARGUS_MODE_B;
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 598b33375cf..a9339763700 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
@@ -47,7 +47,9 @@ extern "C" {
  *
  * @brief		The main module of the API from the AFBR-S50 SDK.
  *
- * @details		General API for the AFBR-S50 time-of-flight sensor device family.
+ * @details		General API for the AFBR-S50 time-of-flight sensor device family.\n
+ * 				See the \ref getting_started Guide for a detailed description
+ * 				on how to use the module/API.
  *
  * @addtogroup 	argusapi
  * @{
@@ -120,7 +122,7 @@ status_t Argus_Reinit(argus_hnd_t *hnd);
  * @brief 	Deinitializes the API modules and the device.
  *
  * @details The function deinitializes the device and clear all internal states.
- * 			Can be used to cleanup before releaseing the memory. The device
+ * 			Can be used to cleanup before releasing the memory. The device
  * 			can not be used any more and must be initialized again prior to next
  * 			usage.
  *
@@ -149,10 +151,19 @@ status_t Argus_Deinit(argus_hnd_t *hnd);
  * 			In order to implement an individual memory allocation method,
  * 			define and implement the following weakly binded method and return
  * 			a pointer to the newly allocated memory. *
+ *
  * 			@code void * Argus_Malloc (size_t size) @endcode
+ *
  * 			Also see the #Argus_DestroyHandle method for the corresponding
  * 			deallocation of the allocated memory.
  *
+ * @note	Although the method is using memory allocated on the heap, it
+ * 			is eventually no dynamic memory allocation, since the block of
+ * 			memory is kept all the time and no memory blocks are dynamically
+ * 			freed and re-allocated. If the usage of heap must be avoided, one
+ * 			can always implement its own version of the `Argus_Malloc` function
+ * 			to create the memory elsewhere.
+ *
  * @return 	Returns a pointer to the newly allocated device handler object.
  * 			Returns a null pointer if the allocation failed!
  *****************************************************************************/
@@ -176,6 +187,9 @@ argus_hnd_t *Argus_CreateHandle(void);
  *
  * 			@code void Argus_Free (void * ptr) @endcode
  *
+ * 			Also see the #Argus_CreateHandle method for the corresponding
+ * 			allocation of the required memory.
+ *
  * @param	hnd The device handle object to be deallocated.
  *****************************************************************************/
 void Argus_DestroyHandle(argus_hnd_t *hnd);
@@ -269,46 +283,50 @@ s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd);
 /*!***************************************************************************
  * @brief	Starts the timer based measurement cycle asynchronously.
  *
- * @details This function starts a timer based measurement cycle asynchronously.
+ * @details This function starts a timer based measurement cycle asynchronously
  * 			in the background. A periodic timer interrupt triggers the measurement
- * 			frames on the ASIC and the data readout afterwards. When the frame is
- * 			finished, a callback (which is passed as a parameter to the function)
- * 			is invoked in order to inform the main thread to call the \link
- * 			#Argus_EvaluateData data evaluation method\endlink. This call is
- * 			mandatory to release the data buffer for the next measurement cycle
- * 			and it must not be invoked from the callback since it is within an
- * 			interrupt service routine. Rather a flag should inform the main thread
- * 			to invoke the evaluation as soon as possible in order to not introduce
- * 			any unwanted delays to the next measurement frame.
+ * 			frames on the ASIC and the data readout afterwards.
+ *
+ * 			When the measurement has finished, a callback (which is passed as
+ * 			a parameter to the function) is invoked in order to inform the
+ * 			main thread to call the \link #Argus_EvaluateData data evaluation
+ * 			method\endlink. This call is mandatory to release the data buffer
+ * 			for the next measurement and it must not be invoked directly from
+ * 			the callback since it is currently within an interrupt service
+ * 			routine. Rather a flag should inform the main thread or task
+ * 			scheduler to invoke the evaluation as soon as possible in order
+ * 			to not introduce any unwanted delays to the next measurement frame.
+ *
  *			The next measurement frame will be started as soon as the pre-
  *			conditions are meet. These are:
  *			 1. timer flag set (i.e. a certain time has passed since the last
  *			 	measurement in order to fulfill eye-safety),
  *			 2. device idle (i.e. no measurement currently ongoing) and
  *			 3. data buffer ready (i.e. the previous data has been evaluated).
+ *
  *			Usually, the device idle and data buffer ready conditions are met
  *			before the timer tick occurs and thus the timer dictates the frame
  *			rate.
  *
- * 			The callback function pointer will be invoked when the measurement
- * 			frame has finished successfully or whenever an error, that cannot
- * 			be handled internally, occurs.
+ *			The callback function pointer will be invoked when the measurement
+ *			frame has finished successfully or whenever an error, that cannot
+ *			be handled internally, occurs.
  *
- * 			The periodic timer interrupts are used to check the measurement status
+ *			The periodic timer interrupts are used to check the measurement status
  *			for timeouts. An error is invoked when a measurement cycle have not
  *			finished within the specified time.
  *
  *			Use #Argus_StopMeasurementTimer to stop the measurements.
  *
  * @note	In order to use this function, the periodic interrupt timer module
- * 			(see @ref argus_timer) must be implemented!
+ *			(see @ref argus_timer) must be implemented!
  *
  * @param	hnd The API handle; contains all internal states and data.
  * @param	cb  Callback function that will be invoked when the measurement
- * 				  is completed. Its parameters are the \link #status_t status
- * 				  \endlink and a pointer to the \link #argus_results_t results
- * 				  \endlink structure. If an error occurred, the status differs
- * 				  from #STATUS_OK and the second parameter is null.
+ *				  is completed. Its parameters are the \link #status_t status
+ *				  \endlink and a pointer to the \link #argus_results_t results
+ *				  \endlink structure. If an error occurred, the status differs
+ *				  from #STATUS_OK and the second parameter is null.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, argus_callback_t cb);
@@ -329,15 +347,19 @@ status_t Argus_StopMeasurementTimer(argus_hnd_t *hnd);
  * @brief	Triggers a single measurement frame asynchronously.
  *
  * @details This function immediately triggers a single measurement frame
- * 			asynchronously if all the pre-conditions are met. Otherwise it returns
- * 			with a corresponding status.
- *			When the frame is finished, a callback (which is passed as a parameter
- *			to the function) is invoked in order to inform the main thread to
- *			call the \link #Argus_EvaluateData data evaluation method\endlink.
- *			This call is mandatory to release the data buffer for the next
- *			measurement and it must not be invoked from the callback since it is
- *			within an interrupt service routine. Rather a flag should inform
- *			the main thread to invoke the evaluation.
+ *			asynchronously if all the pre-conditions are met. Otherwise it
+ *			returns with a corresponding status (e.g. #STATUS_BUSY or
+ *			#STATUS_ARGUS_POWERLIMIT).
+ *
+ *			When the measurement has finished, a callback (which is passed as
+ *			a parameter to the function) is invoked in order to inform the
+ *			main thread to call the \link #Argus_EvaluateData data evaluation
+ *			method\endlink. This call is mandatory to release the data buffer
+ *			for the next measurement and it must not be invoked directly from
+ *			the callback since it is currently within an interrupt service
+ *			routine. Rather a flag should inform the main thread or task
+ *			scheduler to invoke the evaluation task.
+ *
  *			The pre-conditions for starting a measurement frame are:
  *			 1. timer flag set (i.e. a certain time has passed since the last
  *			 	measurement in order to fulfill eye-safety),
@@ -373,22 +395,23 @@ status_t Argus_Abort(argus_hnd_t *hnd);
 /*!***************************************************************************
  * @brief 	Checks the state of the device/driver.
  *
- * @details	Returns the current module state:
+ * @details	Returns the current module status or error if any.
+ * 			See the following for a list of errors:
  *
  *			Status:
  *			- Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE).
- * 			- Busy: Device or SPI interface are busy (== #STATUS_BUSY).
- * 			- Initializing: The modules and devices are currently initializing
- * 			                (== #STATUS_INITIALIZING).
- * 			.
+ *			- Busy: Device or SPI interface are busy (== #STATUS_BUSY).
+ *			- Initializing: The modules and devices are currently initializing
+ *			                (== #STATUS_INITIALIZING).
+ *			.
  *
- * 			Error:
- * 			- Not Initialized: The modules (or any submodule) has not been
- * 							   initialized yet (== #ERROR_NOT_INITIALIZED).
- * 			- Not Connected: No device has been connected (or connection errors
- * 							 have occured) (== #ERROR_ARGUS_NOT_CONNECTED).
- * 			- Timeout: A previous frame measurement has not finished within a
- * 					   specified time (== #ERROR_TIMEOUT).
+ *			Error:
+ *			- Not Initialized: The modules (or any submodule) has not been
+ *							   initialized yet (== #ERROR_NOT_INITIALIZED).
+ *			- Not Connected: No device has been connected (or connection errors
+ *							 have occurred) (== #ERROR_ARGUS_NOT_CONNECTED).
+ *			- Timeout: A previous frame measurement has not finished within a
+ *					   specified time (== #ERROR_TIMEOUT).
  *			.
  *
  * @param	hnd The API handle; contains all internal states and data.
@@ -412,21 +435,22 @@ status_t Argus_Ping(argus_hnd_t *hnd);
  * @brief 	Evaluate useful information from the raw measurement data.
  *
  * @details	This function is called with a pointer to the raw results obtained
- * 			from the measurement cycle. It evaluates this data and creates
- * 			useful information from it. Furthermore, calibration is applied to
- * 			the data. Finally, the results are used in order to adapt the device
- * 			configuration to the ambient conditions in order to achieve optimal
- * 			device performance.\n
- * 			Therefore, it consists of the following sub-functions:
- * 			- Apply pre-calibration: Applies calibration steps before evaluating
- * 			  the data, i.e. calculations that are to the integration results
- * 			  directly.
- * 			- Evaluate data: Calculates measurement parameters such as range,
- * 			  amplitude or ambient light intensity, depending on the configurations.
- * 			- Apply post-calibration: Applies calibrations after evaluation of
- * 			  measurement data, i.e. calibrations applied to the calculated
- * 			  values such as range.
- * 			- Dynamic Configuration Adaption: checks if the configuration needs
+ *			from the measurement cycle. It evaluates this data and creates
+ *			useful information from it. Furthermore, calibration is applied to
+ *			the data. Finally, the results are used in order to adapt the device
+ *			configuration to the ambient conditions in order to achieve optimal
+ *			device performance.
+ *
+ *			Therefore, it consists of the following sub-functions:
+ *			- Apply pre-calibration: Applies calibration steps before evaluating
+ *			  the data, i.e. calculations that are to the integration results
+ *			  directly.
+ *			- Evaluate data: Calculates measurement parameters such as range,
+ *			  amplitude or ambient light intensity, depending on the configurations.
+ *			- Apply post-calibration: Applies calibrations after evaluation of
+ *			  measurement data, i.e. calibrations applied to the calculated
+ *			  values such as range.
+ *			- Dynamic Configuration Adaption: checks if the configuration needs
  *			  to be adjusted before the next measurement cycle in order to
  *			  achieve optimum performance. Note that the configuration might not
  *			  applied directly but before the next measurement starts. This is
@@ -506,10 +530,10 @@ status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd, argus_mode_t mo
  *			  relative pixel offset w.r.t. the average range is also compensated.
  *			.
  *
- * 			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.
+ *			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.
  *
  * @param	hnd The API handle; contains all internal states and data.
  * @param	mode The targeted measurement mode.
@@ -556,10 +580,10 @@ status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd,
  *			  relative pixel offset w.r.t. the average range is also compensated.
  *			.
  *
- * 			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.
+ *			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.
  *
  * @param	hnd The API handle; contains all internal states and data.
  * @param	mode The targeted measurement mode.
@@ -684,6 +708,29 @@ status_t Argus_SetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd,
 status_t Argus_GetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd,
 		argus_mode_t mode,
 		argus_snm_mode_t *value);
+#if 0
+///*!***************************************************************************
+// * @brief 	Sets the Crosstalk Monitor (XTM) mode to a specified device.
+// * @param	hnd The API handle; contains all internal states and data.
+// * @param	mode The targeted measurement mode.
+// * @param	value The new XTM mode value (true: enabled; false: disabled).
+// * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
+// *****************************************************************************/
+//status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd,
+//													argus_mode_t mode,
+//													bool value);
+//
+///*!***************************************************************************
+// * @brief 	Gets the Crosstalk Monitor (XTM) mode from a specified device.
+// * @param	hnd The API handle; contains all internal states and data.
+// * @param	mode The targeted measurement mode.
+// * @param	value The current XTM mode value (true: enabled; false: disabled).
+// * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
+// *****************************************************************************/
+//status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd,
+//													argus_mode_t mode,
+//													bool * value);
+#endif
 
 /*!***************************************************************************
  * @brief 	Sets the full DCA module configuration to a specified device.
@@ -755,12 +802,12 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd,
  *
  * @param	hnd The API handle; contains all internal states and data.
  * @param	mode The targeted measurement mode.
- * @param	value The new global range offset in meter and Q9.22 format.
+ * @param	value The new global range offset 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,
 		argus_mode_t mode,
-		q9_22_t value);
+		q0_15_t value);
 
 /*!***************************************************************************
  * @brief 	Gets the global range offset value from a specified device.
@@ -769,12 +816,12 @@ status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd,
  *
  * @param	hnd The API handle; contains all internal states and data.
  * @param	mode The targeted measurement mode.
- * @param	value The current global range offset in meter and Q9.22 format.
+ * @param	value The current global range offset 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,
 		argus_mode_t mode,
-		q9_22_t *value);
+		q0_15_t *value);
 
 /*!***************************************************************************
  * @brief 	Sets the relative pixel offset table to a specified device.
@@ -922,22 +969,30 @@ void Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X]
 		argus_mode_t mode);
 
 /*!***************************************************************************
- * @brief 	Sets the sample count for the range offset calibration sequence.
+ * @brief 	Sets the sample time for the range offset calibration sequence.
+ *
+ * @details	Gets the measurement sample acquisition time for executing the
+ * 			range offset calibration sequence and generate the offset data.\n
+ * 			Units: msec.
  *
  * @param	hnd The API handle; contains all internal states and data.
- * @param	value The new range offset calibration sequence sample count.
+ * @param	value The new range offset calibration sequence sample time.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_SetCalibrationRangeOffsetSequenceSampleCount(argus_hnd_t *hnd, uint16_t value);
+status_t Argus_SetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t value);
 
 /*!***************************************************************************
- * @brief 	Gets the sample count for the range offset calibration sequence.
+ * @brief 	Gets the sample time for the range offset calibration sequence.
+ *
+ * @details	Gets the measurement sample acquisition time for executing the
+ * 			range offset calibration sequence and generate the ooffset data.\n
+ * 			Units: msec.
  *
  * @param	hnd The API handle; contains all internal states and data.
- * @param	value The current range offset calibration sequence sample count.
+ * @param	value The current range offset calibration sequence sample time.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_GetCalibrationRangeOffsetSequenceSampleCount(argus_hnd_t *hnd, uint16_t *value);
+status_t Argus_GetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value);
 
 /*!***************************************************************************
  * @brief 	Sets the pixel-to-pixel crosstalk compensation parameters to a specified device.
@@ -1067,23 +1122,31 @@ status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd,
 		argus_mode_t mode);
 
 /*!***************************************************************************
- * @brief 	Sets the sample count for the crosstalk calibration sequence.
+ * @brief 	Sets the sample time for the crosstalk calibration sequence.
+ *
+ * @details	Sets the measurement sample acquisition time for executing the
+ * 			crosstalk calibration sequence and generate the crosstalk data.\n
+ * 			Units: msec.
  *
  * @param	hnd The API handle; contains all internal states and data.
- * @param	value The new crosstalk calibration sequence sample count.
+ * @param	value The new crosstalk calibration sequence sample time.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_SetCalibrationCrosstalkSequenceSampleCount(argus_hnd_t *hnd,
+status_t Argus_SetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd,
 		uint16_t value);
 
 /*!***************************************************************************
- * @brief 	Gets the sample count for the crosstalk calibration sequence.
+ * @brief 	Gets the sample time for the crosstalk calibration sequence.
+ *
+ * @details	Gets the measurement sample acquisition time for executing the
+ * 			crosstalk calibration sequence and generate the crosstalk data.\n
+ * 			Units: msec.
  *
  * @param	hnd The API handle; contains all internal states and data.
- * @param	value The current crosstalk calibration sequence sample count.
+ * @param	value The current crosstalk calibration sequence sample time.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
-status_t Argus_GetCalibrationCrosstalkSequenceSampleCount(argus_hnd_t *hnd,
+status_t Argus_GetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd,
 		uint16_t *value);
 
 /*!***************************************************************************
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 f5f68453680..f23d1176484 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
@@ -95,25 +95,19 @@
 #define ARGUS_CFG_DCA_DEPTH_MIN 	((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble
 
 
-/*! The maximum optical output power, i.e. the maximum VCSEL 1 high current in LSB. */
+/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */
 #define ARGUS_CFG_DCA_POWER_MAX_LSB (ADS_LASET_VCSEL_HC1_MASK >> ADS_LASET_VCSEL_HC1_SHIFT)
 
-/*! The minimum optical output power, i.e. the minimum VCSEL 1 high current in mA. */
+/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */
 #define ARGUS_CFG_DCA_POWER_MIN_LSB (1)
 
-/*! The maximum optical output power, i.e. the maximum VCSEL 1 high current in LSB. */
+/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */
 #define ARGUS_CFG_DCA_POWER_MAX		(ADS0032_HIGH_CURRENT_LSB2MA(ARGUS_CFG_DCA_POWER_MAX_LSB + 1))
 
-/*! The minimum optical output power, i.e. the minimum VCSEL 1 high current in mA. */
+/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */
 #define ARGUS_CFG_DCA_POWER_MIN 	(1)
 
 
-
-
-
-
-
-
 /*! The dynamic configuration algorithm Pixel Input Gain stage count. */
 #define ARGUS_DCA_GAIN_STAGE_COUNT (4U)
 
@@ -129,35 +123,32 @@
 
 
 /*! The dynamic configuration algorithm Optical Output Power stage count. */
-#define ARGUS_DCA_POWER_STAGE_COUNT (4U)
+#define ARGUS_DCA_POWER_STAGE_COUNT (2U)
 
 /*! The dynamic configuration algorithm state mask for the Optical Output Power stage. */
-#define ARGUS_STATE_DCA_POWER_MASK (0x03U)
+#define ARGUS_STATE_DCA_POWER_MASK (0x01U)
 
 /*! The dynamic configuration algorithm state mask for the Optical Output Power stage. */
-#define ARGUS_STATE_DCA_POWER_SHIFT (12U)
+#define ARGUS_STATE_DCA_POWER_SHIFT (13U)
 
 /*! Getter for the dynamic configuration algorithm Optical Output Power stage. */
 #define ARGUS_STATE_DCA_POWER_GET(state) \
 	(((state) >> ARGUS_STATE_DCA_POWER_SHIFT) & ARGUS_STATE_DCA_POWER_MASK)
 
 
-/*! The dynamic configuration algorithm state mask for the Max. Analog Integration Depth shift value. */
-#define ARGUS_STATE_DCA_DEPTH_SHFT_MASK (0x0FU)
-
-/*! The dynamic configuration algorithm state mask for the Max. Analog Integration Depth shift value. */
-#define ARGUS_STATE_DCA_DEPTH_SHFT_SHIFT (8U)
-
-/*! Getter for the dynamic configuration algorithm Max. Analog Integration Depth shift value. */
-#define ARGUS_STATE_DCA_DEPTH_SHFT_GET(state) \
-	(((state) >> ARGUS_STATE_DCA_DEPTH_SHFT_SHIFT) & ARGUS_STATE_DCA_DEPTH_SHFT_MASK)
 
 
 /*!***************************************************************************
  * @brief	The dynamic configuration algorithm enable flags.
  *****************************************************************************/
 typedef enum {
-	/*! DCA is disabled and will be completely skipped. */
+	/*! @internal
+	 *
+	 *  DCA is disabled and will be completely skipped.
+	 *
+	 *  @note This state is for internal/debugging usage only as it also
+	 *        disables laser safety checks of the device configuration.
+	 *        An error will occur when used with the API.*/
 	DCA_ENABLE_OFF = 0,
 
 	/*! DCA is enabled and will dynamically adjust the device configuration. */
@@ -168,21 +159,30 @@ typedef enum {
 
 } argus_dca_enable_t;
 
+/*!***************************************************************************
+ * @brief	The DCA amplitude evaluation method.
+ *****************************************************************************/
+typedef enum {
+	/*! Evaluate the DCA amplitude as the maximum of all valid amplitudes. */
+	DCA_AMPLITUDE_MAX = 1U,
+
+	/*! Evaluate the DCA amplitude as the average of all valid amplitudes. */
+	DCA_AMPLITUDE_AVG = 2U,
+
+} argus_dca_amplitude_mode_t;
+
 /*!***************************************************************************
  * @brief	The dynamic configuration algorithm Optical Output Power stages enumerator.
  *****************************************************************************/
 typedef enum {
-	/*! Low output power stage. */
+	/*! Use low output power stage. */
 	DCA_POWER_LOW = 0,
 
-	/*! Medium low output power stage. */
-	DCA_POWER_MEDIUM_LOW = 1,
+	/*! Use high output power stage. */
+	DCA_POWER_HIGH = 1,
 
-	/*! Medium high output power stage. */
-	DCA_POWER_MEDIUM_HIGH = 2,
-
-	/*! High output power stage. */
-	DCA_POWER_HIGH = 3
+	/*! Use low and high output power stages automatically. */
+	DCA_POWER_AUTO = 2
 
 } argus_dca_power_t;
 
@@ -208,108 +208,126 @@ typedef enum {
 /*!***************************************************************************
  * @brief	State flags for the current frame.
  * @details	State flags determine the current state of the measurement frame:
- * 			- [0]: #ARGUS_STATE_MEASUREMENT_MODE: Measurement Mode:
- * 				- 0: Mode A
- * 				- 1: Mode B
- * 				.
- * 			- [1]: #ARGUS_STATE_DUAL_FREQ_MODE: Dual Frequency Mode Enabled Flag
- * 				- 0: Disabled, measurements w/ base frequency only
- * 				- 1: Enabled, measurements w/ detuned frequency
- * 				.
- * 			- [2]: #ARGUS_STATE_MEASUREMENT_FREQ: Measurement Frequency for
- * 			       Dual Frequency Mode, (only valid if #ARGUS_STATE_DUAL_FREQ_MODE
- * 			       flag is set)
- * 				- 0: A-Frame w/ detuned frequency
- * 				- 1: B-Frame w/ detuned frequency
- * 				.
+ * 			- [0]: #ARGUS_STATE_MEASUREMENT_MODE
+ * 			- [1]: #ARGUS_STATE_DUAL_FREQ_MODE
+ * 			- [2]: #ARGUS_STATE_MEASUREMENT_FREQ
  * 			- [3]: #ARGUS_STATE_DEBUG_MODE
- * 			- [4]: #ARGUS_STATE_GOLDEN_PIXEL_MODE
+ * 			- [4]: #ARGUS_STATE_WEAK_SIGNAL
  * 			- [5]: #ARGUS_STATE_BGL_WARNING
  * 			- [6]: #ARGUS_STATE_BGL_ERROR
  * 			- [7]: #ARGUS_STATE_PLL_LOCKED
- * 				- 0: PLL_LOCKED bit was not set at start of integration;
- * 				- 0: PLL_LOCKED bit was set at start of integration;
- * 				.
- * 			- [8-11]: Max. Depth Shift Value
- * 			- [12-13]: Power Stages
- * 			- [14-15]: Gain Stages
+ * 			- [8]: #ARGUS_STATE_LASER_WARNING
+ * 			- [9]: #ARGUS_STATE_LASER_ERROR
+ * 			- [10]: #ARGUS_STATE_HAS_DATA
+ * 			- [11]: #ARGUS_STATE_HAS_AUX_DATA
+ * 			- [12]: #ARGUS_STATE_DCA_MAX
+ * 			- [13]: DCA Power Stage
+ * 			- [14-15]: DCA Gain Stages
  * 			.
  *****************************************************************************/
 typedef enum {
 	/*! No state flag set. */
 	ARGUS_STATE_NONE = 0,
 
-	/*! 0x01: Measurement Mode.
-	 *  - 0: Mode A: Long Range / Medium Precision
-	 *  - 1: Mode B: Short Range / High Precision */
+	/*! 0x0001: Measurement Mode.
+	 *  		- 0: Mode A: Long Range / Medium Precision
+	 *  		- 1: Mode B: Short Range / High Precision */
 	ARGUS_STATE_MEASUREMENT_MODE = 1U << 0U,
 
-	/*! 0x02: Dual Frequency Mode Enabled.
-	 *  - 0: Disabled: measurements with base frequency,
-	 *  - 1: Enabled: measurement with detuned frequency. */
+	/*! 0x0002: Dual Frequency Mode Enabled.
+	 *  		- 0: Disabled: measurements with base frequency,
+	 *  		- 1: Enabled: measurement with detuned frequency. */
 	ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U,
 
-	/*! 0x04: Measurement Frequency for Dual Frequency Mode
-	 *        (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set).
-	 *  - 0: A-Frame w/ detuned frequency,
-	 *  - 1: B-Frame w/ detuned frequency */
+	/*! 0x0004: Measurement Frequency for Dual Frequency Mode
+	 *          (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,
 
-	/*! 0x08: Debug Mode. If set, the range value of erroneous pixels are not
-	 * 		  cleared or reset.
-	 * 	- 0: Disabled (default).
-	 *  - 1: Enabled. */
+	/*! 0x0008: Debug Mode. If set, the range value of erroneous pixels
+	 * 		  	are not cleared or reset.
+	 * 			- 0: Disabled (default).
+	 *  		- 1: Enabled. */
 	ARGUS_STATE_DEBUG_MODE = 1U << 3U,
 
-	/*! 0x10: Golden Pixel Mode Flag.
-	 *  	  Set whenever the Pixel Binning Algorithm is operating in the
-	 *  	  Golden Pixel Mode.
-	 * 	- 0: Normal Pixel Binning Mode.
-	 *  - 1: Golden Pixel Mode. */
-	ARGUS_STATE_GOLDEN_PIXEL_MODE = 1U << 4U,
+	/*! 0x0010: Weak Signal Flag.
+	 * 			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.
+	 * 			- 0: Normal Signal.
+	 *  		- 1: Weak Signal or Golden Pixel Mode. */
+	ARGUS_STATE_WEAK_SIGNAL = 1U << 4U,
 
-	/*! 0x20: Background Light Warning Flag.
-	 *        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. */
+	/*! 0x0020: Background Light Warning Flag.
+	 *        	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,
 
-	/*! 0x40: Background Light Error Flag.
-	 *        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.
-	 *  - 1: Error: Background Light is too high. */
+	/*! 0x0040: Background Light Error Flag.
+	 *        	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.
+	 *  		- 1: Error: Background Light is too high. */
 	ARGUS_STATE_BGL_ERROR = 1U << 6U,
 
-	/*! 0x80: PLL_LOCKED bit.
-	 *  - 0: PLL not locked at start of integration.
-	 *  - 1: PLL locked at start of integration. */
+	/*! 0x0080: PLL_LOCKED bit.
+	 *  		- 0: PLL not locked at start of integration.
+	 *  		- 1: PLL locked at start of integration. */
 	ARGUS_STATE_PLL_LOCKED = 1U << 7U,
 
-	/*! DCA is in low Optical Output Power stage. */
-	ARGUS_STATE_DCA_POWER_LOW = DCA_GAIN_LOW << ARGUS_STATE_DCA_POWER_SHIFT,
+	/*! 0x0100: Laser Failure Warning Flag.
+	 *        	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).
+	 *  		- 0: No Warning: Laser is operating properly.
+	 *  		- 1: Warning: Invalid laser conditions detected. If the invalid
+	 *  		     condition stays, a laser malfunction error is raised. */
+	ARGUS_STATE_LASER_WARNING = 1U << 8U,
 
-	/*! DCA is in medium-low Optical Output Power stage. */
-	ARGUS_STATE_DCA_POWER_MED_LOW = DCA_GAIN_MEDIUM_LOW << ARGUS_STATE_DCA_POWER_SHIFT,
+	/*! 0x0200: Laser Failure Error Flag.
+	 * 			Set whenever a laser malfunction error is raised and the
+	 * 			system is put into a safe state.
+	 *  		- 0: No Error: Laser is operating properly.
+	 *  		- 1: Error: Invalid laser conditions are detected for a certain
+	 *  		     soak time and the system is put into a safe state. */
+	ARGUS_STATE_LASER_ERROR = 1U << 9U,
 
-	/*! DCA is in medium-high Optical Output Power stage. */
-	ARGUS_STATE_DCA_POWER_MED_HIGH = DCA_GAIN_MEDIUM_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
+	/*! 0x0400: Set if current frame has distance measurement data available.
+	 *  		- 0: No measurement data available, all values are 0 or stalled.
+	 *  		- 1: Measurement data is available and correctly evaluated. */
+	ARGUS_STATE_HAS_DATA = 1U << 10U,
 
-	/*! DCA is in high Optical Output Power stage. */
-	ARGUS_STATE_DCA_POWER_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
+	/*! 0x0800: Set if current frame has auxiliary measurement data available.
+	 *  		- 0: No auxiliary data available, all values are 0 or stalled.
+	 *  		- 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,
+
+	/*! 0x2000: DCA is in high Optical Output Power stage. */
+	ARGUS_STATE_DCA_POWER_HIGH = DCA_POWER_HIGH << ARGUS_STATE_DCA_POWER_SHIFT,
 
 	/*! DCA is in low Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_LOW = DCA_GAIN_LOW << ARGUS_STATE_DCA_GAIN_SHIFT,
 
-	/*! DCA is in medium-low Pixel Input Gain stage. */
+	/*! 0x4000: DCA is in medium-low Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_MED_LOW = DCA_GAIN_MEDIUM_LOW << ARGUS_STATE_DCA_GAIN_SHIFT,
 
-	/*! DCA is in medium-high Pixel Input Gain stage. */
+	/*! 0x8000: DCA is in medium-high Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_MED_HIGH = DCA_GAIN_MEDIUM_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT,
 
-	/*! DCA is in high Pixel Input Gain stage. */
+	/*! 0xC000: DCA is in high Pixel Input Gain stage. */
 	ARGUS_STATE_DCA_GAIN_HIGH = DCA_GAIN_HIGH << ARGUS_STATE_DCA_GAIN_SHIFT,
 
 } argus_state_t;
@@ -331,9 +349,7 @@ typedef enum {
 typedef struct {
 	/*! Enables the automatic configuration adaption features.
 	 *  Enables the dynamic part if #DCA_ENABLE_DYNAMIC and the static only if
-	 *  #DCA_ENABLE_STATIC. If set to DCA_ENABLE_OFF, the DCA is completely
-	 *  skipped and the static register values are considered which is
-	 *  recommended for advanced debugging only. */
+	 *  #DCA_ENABLE_STATIC. */
 	argus_dca_enable_t Enabled;
 
 	/*! The threshold value of saturated pixels that causes a linear reduction
@@ -373,17 +389,16 @@ typedef struct {
 	 *  1 <= SatPxThLin <= SatPxThExp <= SatPxThRst <= 33 */
 	uint8_t SatPxThRst;
 
-	/*! The amplitude to be targeted from the lower regime. If the amplitude
-	 *  lower than the target value, a linear increase of integration energy
-	 *  will happen in order to optimize for best performance.
+	/*! The DCA amplitude to be targeted from the lower regime. If the DCA
+	 *  amplitude lower than the target value, a linear increase of integration
+	 *  energy will happen in order to optimize for best performance.
 	 *
-	 *  Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX or 0
-	 *  Set 0 to disable optimization toward the target amplitude.
+	 *  Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX
 	 *  Note further that the following condition must hold:
 	 *  'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */
 	uq12_4_t Atarget;
 
-	/*! The low threshold value for the max. amplitude. If the max. amplitude
+	/*! The low threshold value for the DCA amplitude. If the DCA amplitude
 	 *  falls below this value, the integration depth will be increases.
 	 *
 	 *  Valid values: #ARGUS_CFG_DCA_ATH_MIN, ... #ARGUS_CFG_DCA_ATH_MAX
@@ -391,7 +406,7 @@ typedef struct {
 	 *  'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */
 	uq12_4_t AthLow;
 
-	/*! The high threshold value for the max. amplitude. If the max. amplitude
+	/*! The high threshold value for the DCA amplitude. If the DCA amplitude
 	 *  exceeds this value, the integration depth will be decreases. Note that
 	 *  also saturated pixels will cause a decrease of the integration depth.
 	 *
@@ -400,6 +415,15 @@ typedef struct {
 	 *  'MIN' <= AthLow <= Atarget <= AthHigh <= 'MAX' */
 	uq12_4_t AthHigh;
 
+	/*! The DCA amplitude calculation algorithm. Either maximum
+	 *  (#DCA_AMPLITUDE_MAX) or average (#DCA_AMPLITUDE_AVG) amplitude can be
+	 *  selected. */
+	argus_dca_amplitude_mode_t AmplitudeMode;
+
+	/*! The power stage selector.
+	 *  Selects the used power stages, i.e. LOW, HIGH or AUTO (LOW+HIGH). */
+	argus_dca_power_t Power;
+
 	/*! The nominal analog integration depth in UQ10.6 format,
 	 *  i.e. the nominal pattern count per sample.
 	 *
@@ -424,22 +448,6 @@ typedef struct {
 	 *  'MIN' <= DepthMin <= DepthNom <= DepthMax <= 'MAX' */
 	uq10_6_t DepthMax;
 
-	/*! The nominal optical output power in mA,
-	 *  i.e. the nominal VCSEL_HC1 setting.
-	 *
-	 *  Valid values: #ARGUS_CFG_DCA_POWER_MIN, ... #ARGUS_CFG_DCA_POWER_MAX
-	 *  Note further that the following condition must hold:
-	 *  'MIN' <= PowerMin <= PowerNom <= 'MAX' */
-	uq12_4_t PowerNom;
-
-	/*! The minimum optical output power in mA,
-	 *  i.e. the minimum VCSEL_HC1 setting.
-	 *
-	 *  Valid values: #ARGUS_CFG_DCA_POWER_MIN, ... #ARGUS_CFG_DCA_POWER_MAX
-	 *  Note further that the following condition must hold:
-	 *  'MIN' <= PowerMin <= PowerNom <= 'MAX' */
-	uq12_4_t PowerMin;
-
 	/*! The nominal pixel gain setting, i.e. the setting for
 	 *  nominal/default gain stage.
 	 *
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h
index 4762e1d9cdb..c639922a7d1 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h
@@ -129,7 +129,7 @@ typedef enum {
 
 
 	/*! Reserved for future extensions. */
-	Reserved = 0b111111
+	Reserved = 0x3F
 
 } argus_module_version_t;
 
@@ -149,6 +149,9 @@ typedef enum {
 	/*! 680nm Red VCSEL v1 */
 	LASER_R_V1 = 3,
 
+	/*! 850nm Infra-Red VCSEL v2 /w extended mode. */
+	LASER_H_V2X = 4,
+
 } argus_laser_type_t;
 
 /*!***************************************************************************
@@ -158,14 +161,20 @@ typedef enum {
 	/*! No device connected or not recognized. */
 	ADS0032_NONE = 0,
 
-	/*! ADS0032 v1.0 */
-	ADS0032_V1_0 = 1,
+	/*! ADS0032 v1a */
+	ADS0032_V1A = 1,
 
-	/*! ADS0032 v1.1 */
-	ADS0032_V1_1 = 2,
+	/*! ADS0032 v1b */
+	ADS0032_V1B = 2,
 
-	/*! ADS0032 v1.2 */
-	ADS0032_V1_2 = 3,
+	/*! ADS0032 v1c */
+	ADS0032_V1C = 3,
+
+	/*! ADS0032 v1d */
+	ADS0032_V1D = 4,
+
+	/*! ADS0032 v1e */
+	ADS0032_V1E = 5,
 
 } argus_chip_version_t;
 
@@ -192,11 +201,11 @@ typedef enum {
  * @details	Invoked by the API. The content of the abstract data pointer
  * 			depends upon the context.
  * @param	status The module status that caused the callback. #STATUS_OK if
- * 					 everything was as expected.
- * @param	data   An abstract pointer to an user defined data. This will
- * 					 usually be passed to the function that also takes the
- * 					 callback as an parameter. Otherwise it has a special
- * 					 meaning such as configuration or calibration data.
+ * 				   everything was as expected.
+ * @param	data An abstract pointer to an user defined data. This will usually
+ * 				 be passed to the function that also takes the callback as an
+ * 				 parameter. Otherwise it has a special meaning such as
+ * 				 configuration or calibration data.
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 typedef status_t (*argus_callback_t)(status_t status, void *data);
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h
index c5ec32dfa8a..b2517182f87 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h
@@ -39,7 +39,7 @@
 
 /*!***************************************************************************
  * @defgroup 	argusdfm Dual Frequency Mode
- * @ingroup		argusdev
+ * @ingroup		argusapi
  *
  * @brief		Dual Frequency Mode (DFM) parameter definitions and API functions.
  *
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
new file mode 100644
index 00000000000..64588d25f15
--- /dev/null
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h
@@ -0,0 +1,253 @@
+/*************************************************************************//**
+ * @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_MAP_H
+#define ARGUS_MAP_H
+
+/*!***************************************************************************
+ * @defgroup	argusmap ADC Channel Mapping
+ * @ingroup		argusres
+ *
+ * @brief		Pixel ADC Channel Mapping
+ *
+ * @details		The ADC Channels of each pixel or auxiliary channel on the device
+ * 				are numbered in a way that is convenient on the chip architecture.
+ * 				The macros in this module are defined in order to map between the
+ * 				chip internal channel number (ch) to the two-dimensional
+ * 				x-y-indices or one-dimensional n-index representation.
+ *
+ * @addtogroup 	argusmap
+ * @{
+ *****************************************************************************/
+
+#include "api/argus_def.h"
+#include "utility/int_math.h"
+
+
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel ADC channel number from the x-z-indices.
+ * @param	x The x-index of the pixel.
+ * @param	y The y-index of the pixel.
+ * @return	The ADC channel number of the pixel.
+ ******************************************************************************/
+#define PIXEL_XY2CH(x, y) ((((y) << 3U) & 0x10U) | (((x) ^ 0x07U) << 1U) | ((y) & 0x01U))
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel x-index from the ADC channel number.
+ * @param	c The ADC channel number of the pixel.
+ * @return	The x-index of the pixel.
+ ******************************************************************************/
+#define PIXEL_CH2X(c) ((((c) >> 1U) ^ 0x07U) & 0x07U)
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel y-index from the ADC channel number.
+ * @param	c The ADC channel number of the pixel.
+ * @return	The y-index of the pixel.
+ ******************************************************************************/
+#define PIXEL_CH2Y(c) ((((c) >> 3U) & 0x02U) | ((c) & 0x01U))
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the n-index from the x-y-indices.
+ * @param	x The x-index of the pixel.
+ * @param	y The y-index of the pixel.
+ * @return	The n-index of the pixel.
+ ******************************************************************************/
+#define PIXEL_XY2N(x, y) (((x) << 2U) | (y))
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel x-index from the n-index.
+ * @param	n The n-index of the pixel.
+ * @return	The x-index number of the pixel.
+ ******************************************************************************/
+#define PIXEL_N2X(n) ((n) >> 2U)
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel y-index from the n-index.
+ * @param	n The n-index of the pixel.
+ * @return	The y-index number of the pixel.
+ ******************************************************************************/
+#define PIXEL_N2Y(n) ((n) & 0x03U)
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel n-index from the ADC channel number.
+ * @param	n The n-index of the pixel.
+ * @return	The ADC channel number of the pixel.
+ ******************************************************************************/
+#define PIXEL_N2CH(n) ((((n) << 3U) & 0x10U) | ((((n) >> 1U) ^ 0x0EU) & 0x0EU) | ((n) & 0x01U))
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the pixel
+ * @param	c The ADC channel number of the pixel.
+ * @return	The n-index of the pixel.
+ ******************************************************************************/
+#define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U))
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine if a pixel given by the n-index is enabled in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	n n-index of the pixel.
+ * @return 	True if the pixel (n) is enabled.
+ ******************************************************************************/
+#define PIXELN_ISENABLED(msk, n) (((msk) >> (n)) & 0x01U)
+
+/*!*****************************************************************************
+ * @brief	Macro to enable 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 enable.
+ ******************************************************************************/
+#define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (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))))
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine if an ADC pixel channel is enabled from a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The ADC channel number of the pixel.
+ * @return 	True if the specified pixel ADC channel is enabled.
+ ******************************************************************************/
+#define PIXELCH_ISENABLED(msk, c) (PIXELN_ISENABLED(msk, PIXEL_CH2N(c)))
+
+/*!*****************************************************************************
+ * @brief	Macro to enable an ADC pixel channel in a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The pixel ADC channel number to enable.
+ ******************************************************************************/
+#define PIXELCH_ENABLE(msk, c) (PIXELN_ENABLE(msk, PIXEL_CH2N(c)))
+
+/*!*****************************************************************************
+ * @brief	Macro to disable an ADC pixel channel in a pixel mask.
+ * @param	msk The 32-bit pixel mask
+ * @param	c The pixel ADC channel number to disable.
+ ******************************************************************************/
+#define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c)))
+
+
+/*!*****************************************************************************
+ * @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
+ * @param	x x-index of the pixel.
+ * @param	y y-index of the pixel.
+ * @return 	True if the pixel (x,y) is enabled.
+ ******************************************************************************/
+#define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y)))
+
+/*!*****************************************************************************
+ * @brief	Macro to enable a pixel given by the x-y-indices in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	x x-index of the pixel to enable.
+ * @param	y y-index of the pixel to enable.
+ ******************************************************************************/
+#define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y)))
+
+/*!*****************************************************************************
+ * @brief	Macro disable a pixel given by the x-y-indices in a pixel mask.
+ * @param	msk 32-bit pixel mask
+ * @param	x x-index of the pixel to disable.
+ * @param	y y-index of the pixel to disable.
+ ******************************************************************************/
+#define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y)))
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine if an ADC channel is enabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel.
+ * @return 	True if the ADC channel is enabled.
+ ******************************************************************************/
+#define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U)
+
+/*!*****************************************************************************
+ * @brief	Macro to determine if an ADC channel is enabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel to enabled.
+ ******************************************************************************/
+#define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U)))
+
+/*!*****************************************************************************
+ * @brief	Macro to determine if an ADC channel is disabled in a channel mask.
+ * @param	msk 32-bit channel mask
+ * @param	ch channel number of the ADC channel to disable.
+ ******************************************************************************/
+#define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U))))
+
+
+/*!*****************************************************************************
+ * @brief	Macro to determine the number of enabled pixel/channels in a mask
+ * 			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))
+
+/*!*****************************************************************************
+ * @brief	Converts a raw ADC channel mask to a x-y-sorted pixel mask.
+ * @param	msk The raw ADC channel mask to be converted.
+ * @return 	The converted x-y-sorted pixel mask.
+ ******************************************************************************/
+static inline uint32_t ChannelToPixelMask(uint32_t msk)
+{
+	uint32_t res = 0;
+
+	for (uint_fast8_t n = 0; n < 32; n += 2) {
+		res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n;
+	}
+
+	return res;
+}
+
+/*! @} */
+#endif /* ARGUS_MAP_H */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h
index e57de35711b..0e074c6a8b0 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h
@@ -71,8 +71,17 @@
  * 			measurement cycle. I.e. the data that defines a measurement frame.
  *****************************************************************************/
 typedef struct {
-	/*! ADC channel enabled mask for the first
-	 *  channels 0 .. 31 (active pixels channels).
+	/*! Frame integration time in microseconds.
+	 *  The integration time determines the measured time between
+	 *  the start signal and the IRQ. Note that this value will be
+	 *  slightly larger than the actual integration time since the
+	 *  watch is started before the SPI transfer and stopped in the
+	 *  IRQ service routine which also might be delayed due to higher
+	 *  priority tasks. */
+	uint32_t IntegrationTime;
+
+	/*! Pixel enabled mask for the 32 pixels sorted
+	 *  by x-y-indices.
 	 *  See [pixel mapping](@ref argusmap) for more
 	 *  details on the pixel mask. */
 	uint32_t PxEnMask;
@@ -80,29 +89,9 @@ typedef struct {
 	/*! ADS channel enabled mask for the remaining
 	 *  channels 31 .. 63 (miscellaneous values).
 	 *  See [pixel mapping](@ref argusmap) for more
-	 *  details on the channel mask. */
+	 *  details on the ADC channel mask. */
 	uint32_t ChEnMask;
 
-	/*! Pattern count per sample in uq10.6 format.
-	 *  Determines the analog integration depth. */
-	uq10_6_t AnalogIntegrationDepth;
-
-	/*! Sample count per phase/frame.
-	 *  Determines the digital integration depth. */
-	uint16_t DigitalIntegrationDepth;
-
-	/*! Laser current per sample in mA.
-	 *  Determines the optical output power. */
-	uq12_4_t OutputPower;
-
-	/*! Charge pump voltage per sample in LSB.
-	 *  Determines the pixel gain.  */
-	uint8_t PixelGain;
-
-	/*! PLL Frequency Offset, caused by temperature
-	 *  compensation, in PLL_INT_PRD LSBs. */
-	int8_t PllOffset;
-
 	/*! The current state of the measurement frame:
 	 *  - Measurement Mode,
 	 *  - A/B Frame,
@@ -112,6 +101,35 @@ typedef struct {
 	 *  - ... */
 	argus_state_t State;
 
+	/*! Pattern count per sample in uq10.6 format.
+	 *  Determines the analog integration depth. */
+	uq10_6_t AnalogIntegrationDepth;
+
+	/*! Sample count per phase/frame.
+	 *  Determines the digital integration depth. */
+	uint16_t DigitalIntegrationDepth;
+
+	/*! Laser Modulation Current per sample in mA.
+	 *  Determines the optical output power. */
+	uq12_4_t OutputPower;
+
+	/*! The amplitude that is evaluated and used in the DCA module. */
+	uq12_4_t DCAAmplitude;
+
+	/*! Laser Bias Current Settings in LSB. */
+	uint8_t BiasCurrent;
+
+	/*! Charge pump voltage per sample in LSB.
+	 *  Determines the pixel gain.  */
+	uint8_t PixelGain;
+
+	/*! PLL Frequency Offset, caused by temperature
+	 *  compensation, in PLL_INT_PRD LSBs. */
+	int8_t PllOffset;
+
+	/*! The current PLL_CTRL_CUR value. */
+	uint8_t PllCtrlCur;
+
 } argus_meas_frame_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 1d4a5c43b9e..07b4853bda7 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
@@ -153,7 +153,7 @@ typedef struct {
 	 *  final range value from the algorithm, for example, the average
 	 *  of all pixels. See #argus_pba_averaging_mode_t for more details
 	 *  about the individual evaluation modes. */
-	argus_pba_averaging_mode_t Mode;
+	argus_pba_averaging_mode_t AveragingMode;
 
 	/*! The Relative amplitude threshold value (in %) of the max. amplitude.
 	 *  Pixels with amplitude below this threshold value are dismissed.
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 2977312d8e0..faa031aeb73 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
@@ -138,6 +138,17 @@ typedef struct {
 
 } argus_pixel_t;
 
+/*!***************************************************************************
+ * @brief Representation of a correlation vector containing sine/cosine components.
+ *****************************************************************************/
+typedef struct {
+	/*! The sine component. */
+	q15_16_t S;
+
+	/*! The cosine component. */
+	q15_16_t C;
+
+} argus_vector_t;
 
 /*! @} */
 #endif /* ARGUS_PX_H */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h
index 468a9136711..f59d8176347 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h
@@ -62,61 +62,67 @@
 
 /*!***************************************************************************
  * @brief	The 1d measurement results data structure.
- * @details The 1d measurement results obtained by the pixel binning algorithm.
+ * @details The 1d measurement results obtained by the Pixel Binning Algorithm.
  *****************************************************************************/
 typedef struct {
 	/*! Raw 1D range value in meter (Q9.22 format). The distance obtained by
-	 *  the pixel binning algorithm from the current measurement frame. */
+	 *  the Pixel Binning Algorithm from the current measurement frame. */
 	q9_22_t Range;
 
 	/*! The 1D amplitude in LSB (Q12.4 format). The (maximum) amplitude obtained
-	 *  by the pixel binning algorithm from the current measurement frame.
+	 *  by the Pixel Binning Algorithm from the current measurement frame.\n
 	 *  Special value: 0 == No/Invalid Result. */
 	uq12_4_t Amplitude;
 
+	/*! The current signal quality metric of the 1D range value in percentage:\n
+	 *  - 0: n/a,
+	 *  - 1: bad signal,
+	 *  - 100: good signal. */
+	uint8_t SignalQuality;
+
 } argus_results_bin_t;
 
 /*!***************************************************************************
  * @brief	The auxiliary measurement results data structure.
- * @details The auxiliary measurement results obtained by the auxiliary task.
+ * @details	The auxiliary measurement results obtained by the auxiliary task.\n
  * 			Special values, i.e. 0xFFFFU, indicate no readout value available.
  *****************************************************************************/
 typedef struct {
-	/*! VDD ADC channel readout value.
-	 *  Special Value if no value has been measured:
+	/*! VDD ADC channel readout value.\n
+	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t VDD;
 
-	/*! Temperature sensor ADC channel readout value.
-	 *  Special Value if no value has been measured:
+	/*! Temperature sensor ADC channel readout value.\n
+	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0x7FFFU (Q11_4_MAX) */
 	q11_4_t TEMP;
 
-	/*! Substrate Voltage ADC Channel readout value.
-	 *  Special Value if no value has been measured:
+	/*! Substrate Voltage ADC Channel readout value.\n
+	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t VSUB;
 
-	/*! VDD VCSEL ADC channel readout value.
-	 *  Special Value if no value has been measured:
+	/*! VDD VCSEL ADC channel readout value.\n
+	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t VDDL;
 
-	/*! APD current ADC Channel readout value.
-	 *  Special Value if no value has been measured:
+	/*! APD current ADC Channel readout value.\n
+	 *  Special Value if no value has been measured:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t IAPD;
 
 	/*! Background Light Value in arbitrary. units,
-	 *  estimated by the substrate voltage control task.
-	 *  Special Value if no value is available:
+	 *  estimated by the substrate voltage control task.\n
+	 *  Special Value if no value is available:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t BGL;
 
 	/*! Shot Noise Amplitude in LSB units,
 	 *  estimated by the shot noise monitor task from
-	 *  the average amplitude of the passive pixels.
-	 *  Special Value if no value is available:
+	 *  the average amplitude of the passive pixels.\n
+	 *  Special Value if no value is available:\n
 	 *  Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */
 	uq12_4_t SNA;
 
@@ -124,7 +130,20 @@ typedef struct {
 
 /*!***************************************************************************
  * @brief	The measurement results data structure.
- * @details Measurement data from the device.
+ * @details This structure contains all information obtained by a single
+ *			distance measurement on the device:
+ *			 - The measurement status can be read from the #Status.
+ *			 - A timing information is given via the #TimeStamp.
+ *			 - Information about the frame state is in the #Frame structure.
+ *			 - The 1D distance results are gathered under #Bin.
+ *			 - The 3D distance results for each pixel is at #Pixels or #Pixel.
+ *			 - Auxiliary values such as temperature can be found at #Auxiliary.
+ *			 - Raw data from the device is stored in the #Data array.
+ *			 .
+ *
+ *			The pixel x-y orientation is sketched in the following graph. Note that
+ *			the laser source would be on the right side beyond the reference pixel.
+ *			See also \link argusmap ADC Channel Mapping\endlink
  * @code
  * 			// Pixel Field: Pixel[x][y]
  * 			//
@@ -149,21 +168,49 @@ typedef struct {
 	/*! The configuration for the current measurement frame. */
 	argus_meas_frame_t Frame;
 
-	/*! Raw unmapped ADC results from the device. */
+	/*! Raw x-y-sorted ADC results from the device.\n
+	 *  Data is arranged as 32-bit values in following order:
+	 *  index > phase; where index is pixel number n and auxiliary ADC channel.\n
+	 *  Note that disabled pixels are skipped.\n
+	 *  e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */
 	uint32_t Data[ARGUS_RAW_DATA_VALUES];
 
-	/*! Raw Range Values from the device in meter.
-	 *  It is the actual distance before software adjustments/calibrations. */
-	argus_pixel_t PixelRef;
+	union {
+		/*! Pixel data indexed by channel number n.\n
+		 *  Contains calibrated range, amplitude and pixel status among others.
+		 *
+		 *  Index n:
+		 *  - 0..31: active pixels
+		 *  - 32:    reference pixel
+		 *
+		 *  See also \link argusmap ADC Channel Mapping\endlink */
+		argus_pixel_t Pixels[ARGUS_PIXELS + 1U];
 
-	/*! Raw Range Values from the device in meter.
-	 *  It is the actual distance before software adjustments/calibrations. */
-	argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
+		struct {
+			/*! Pixel data indexed by x-y-indices.\n
+			 *  The pixels are ordered in a two dimensional array that represent
+			 *  the x and y indices of the pixel.\n
+			*	See also \link argusmap ADC Channel Mapping\endlink
+			 *
+			 *  Contains calibrated range, amplitude and pixel status among others. */
+			argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y];
 
-	/*! Pixel binned results. */
+			/*! Pixel data of the reference pixel.\n
+			 *  The reference pixel is an additional pixel that is located at the TX
+			 *  side in order to monitor the health state of the laser output source.
+			 *  It is mainly used to verify normal operation of the laser source and
+			 *  preventing the system from emitting continuous laser light that exceeds
+			 *  the laser safety limits.
+			 *
+			 *  Contains calibrated range, amplitude and pixel status among others. */
+			argus_pixel_t PixelRef;
+		};
+	};
+
+	/*! The 1D measurement data, obtained by the the Pixel Binning Algorithm. */
 	argus_results_bin_t Bin;
 
-	/*! The auxiliary ADC channel data. */
+	/*! The auxiliary ADC channel data, e.g. sensor temperature. */
 	argus_results_aux_t Auxiliary;
 
 } argus_results_t;
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h
index 2bb6ad0a0db..27763081439 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h
@@ -39,7 +39,7 @@
 
 /*!***************************************************************************
  * @defgroup 	argussnm Shot Noise Monitor
- * @ingroup		argusdev
+ * @ingroup		argusapi
  *
  * @brief		Shot Noise Monitor (SNM) parameter definitions and API functions.
  *
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 77ac2fcafcc..244ad1beec7 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
@@ -128,7 +128,7 @@ enum Status {
 	ERROR_S2PI_INVALID_STATE = -53,
 
 	/*! -54: The specified baud rate is not valid. */
-	ERROR_S2PI_INVALID_BAUD_RATE = -54,
+	ERROR_S2PI_INVALID_BAUDRATE = -54,
 
 	/*! -55: The specified slave identifier is not valid. */
 	ERROR_S2PI_INVALID_SLAVE = -55,
@@ -158,12 +158,9 @@ enum Status {
 	 *  limitations. */
 	STATUS_ARGUS_POWERLIMIT = 105,
 
-	/*! 107: AFBR-S50 Status: No valid signal was detected at any active pixel
-	 * via the Pixel Binning Algorithm. The Golden Pixel was Choosen as a
-	 * fallback value that is consider to be the last pixel that has a valid
-	 * signal for low reflective (or far away) objects.
-	 * The current results should be considered carefully. */
-	STATUS_ARGUS_UNDERFLOW = 107,
+	/*! 106: AFBR-S50 Status: The PLL was not locked when the measurement was
+	 *  started. The measurement frequency and this the range might be off. */
+	STATUS_ARGUS_PLL_NOT_LOCKED = 106,
 
 	/*! 108: AFBR-S50 Status: No object was detected within the field-of-view
 	 *  and measurement range of the device. */
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 2a2ca9d62c9..ea16342c843 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
@@ -53,13 +53,13 @@
 #define ARGUS_API_VERSION_MAJOR    1
 
 /*! Minor version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_MINOR    2
+#define ARGUS_API_VERSION_MINOR    3
 
 /*! Bugfix version number of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUGFIX   3
+#define ARGUS_API_VERSION_BUGFIX   5
 
 /*! Build version nunber of the AFBR-S50 API. */
-#define ARGUS_API_VERSION_BUILD    "20201120091253"
+#define ARGUS_API_VERSION_BUILD    "20210812171515"
 
 /*****************************************************************************/
 
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h
index 0a9e11241e3..3eb7d2cfd4a 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h
@@ -48,7 +48,7 @@ extern "C" {
  * @brief		Global Interrupt Control Layer
  *
  * @details		This module provides functionality to globally enable/disable
- *				interrupts by turning the I-bit in the CPSR on/off.
+ *				interrupts in a nested way.
  *
  *				Here is a simple example implementation using the CMSIS functions
  *				"__enable_irq()" and "__disable_irq()". An integer counter is
@@ -84,6 +84,10 @@ extern "C" {
  * 				instruction only. It does NOT lock interrupts for considerable
  * 				amounts of time.
  *
+ * @note 		The IRQ_LOCK might get called multiple times. Therefore, the
+ * 				API expects that the IRQ_UNLOCK must be called as many times as
+ * 				the IRQ_LOCK was called before the interrupts are enabled.
+ *
  * @note		The interrupts utilized by the AFBR-S50 API can be interrupted
  * 				by other, higher prioritized interrupts, e.g. some system
  * 				critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK
@@ -100,16 +104,22 @@ extern "C" {
 /*!***************************************************************************
  * @brief	Enable IRQ Interrupts
  *
- * @details	Enables IRQ interrupts by clearing the I-bit in the CPSR.
- * 			Can only be executed in Privileged modes.
+ * @details	Enables IRQ interrupts and enters an atomic or critical section.
+ *
+ * @note 	The IRQ_LOCK might get called multiple times. Therefore, the
+ * 			API expects that the IRQ_UNLOCK must be called as many times as
+ * 			the IRQ_LOCK was called before the interrupts are enabled.
  *****************************************************************************/
 void IRQ_UNLOCK(void);
 
 /*!***************************************************************************
  * @brief	Disable IRQ Interrupts
  *
- * @details	Disables IRQ interrupts by setting the I-bit in the CPSR.
- * 			Can only be executed in Privileged modes.
+ * @details	Disables IRQ interrupts and leaves the atomic or critical section.
+ *
+ * @note 	The IRQ_LOCK might get called multiple times. Therefore, the
+ * 			API expects that the IRQ_UNLOCK must be called as many times as
+ * 			the IRQ_LOCK was called before the interrupts are enabled.
  *****************************************************************************/
 void IRQ_LOCK(void);
 
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h
index 0c0bf237cb0..69939b77598 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h
@@ -126,7 +126,6 @@ status_t NVM_Write(uint32_t offset, uint32_t size, uint8_t const *buf);
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t NVM_Read(uint32_t offset, uint32_t size, uint8_t *buf);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h
index 3c17096e8ee..4b8f5331b50 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h
@@ -54,7 +54,7 @@ extern "C" {
  *
  *				**SPI interface:**
  *
- *				The SPI interface is based on a single function:
+ *				The SPI interface is based around a single functionality:
  *
  *				#S2PI_TransferFrame. This function transfers a specified number
  *				of bytes via the interfaces MOSI line and simultaneously reads
@@ -72,32 +72,34 @@ extern "C" {
  *
  *				**GPIO interface:**
  *
- *				The GPIO interface handles the measurement finished interrupt
+ *				The GPIO part of the S2PI interface has two distinct concerns:
+ *
+ *				First, the GPIO interface handles the measurement finished interrupt
  *				from the device. When the device invokes the interrupt, it pulls
  *				the interrupt line to low. Thus the interrupt must trigger when
  *				a transition from high to low occurs on the interrupt line.
  *
- *				The module simply invokes a callback when this interrupt the
- *				pending. The #S2PI_SetIrqCallback method is	used to install the
- *				callback for a specified slave. Each slave will have its own
- *				interrupt line. An additional callback parameter can be set that
- *				would be passed to the callback function.
+ *				The module simply invokes a callback when this interrupt occurs.
+ *				The #S2PI_SetIrqCallback method is used to install the callback
+ *				for a specified slave. Each slave will have its own interrupt
+ *				line. An additional callback parameter can be set that would be
+ *				passed to the callback function.
  *
  *				In addition to the interrupt, all SPI pins need to be accessible
- *				as GPIO pins through the interface. One basic operation would
- *				be to cycle the chip select pin which resets the device.
- *				Additional, the device contains an EEPROM that is connected to
- *				the SPI pins but requires a different protocol that is not
- *				compatible to any standard SPI interface. Therefore, the
- *				interface provides the possibility to switch to GPIO control
- *				that allows to emulate the EEPROM protocol via software bit
- *				banging. Two methods are provided to switch forth and back
- *				between SPI and GPIO control. In GPIO mode, several functions
- *				are used to read and write the individual GPIO pins.
+ *				as GPIO pins through this interface. This is required to read
+ *				the EEPROM memory on the device hat is connected to the SPI
+ *				pins but requires a different protocol that is not compatible
+ *				to any standard SPI interface. Therefore, the interface provides
+ *				the possibility to switch to GPIO control mode that allows to
+ *				emulate the EEPROM protocol via software bit banging.
+ *
+ *				Two methods are provided to switch forth and back between SPI
+ *				and GPIO control. In GPIO mode, several functions are used to
+ *				read and write the individual GPIO pins.
  *
  *				Note that the GPIO mode is only required to readout the EEPROM
- *				at initialization of the device, i.e. during execution of the
- *				#Argus_Init or #Argus_Reinit method. The GPIO mode is not used
+ *				upon initialization of the device, i.e. during execution of the
+ *				#Argus_Init or #Argus_Reinit methods. The GPIO mode is not used
  *				during measurements.
  *
  *
@@ -241,7 +243,9 @@ status_t S2PI_Abort(void);
  * 			 - #STATUS_OK: Successfully installation of the callback.
  *         	 - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided.
  *****************************************************************************/
-status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData);
+status_t S2PI_SetIrqCallback(s2pi_slave_t slave,
+			     s2pi_irq_callback_t callback,
+			     void *callbackData);
 
 /*!***************************************************************************
  * @brief	Reads the current status of the IRQ pin.
@@ -339,11 +343,10 @@ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value);
  *
  * @param	slave The specified S2PI slave.
  * @param	pin The specified S2PI pin.
- * @param	value The GPIO pin state to read (0 = low, 1 = high).
+ * @param	value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level).
  * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
  *****************************************************************************/
 status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h
index 0390020aff8..80deb42016f 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h
@@ -70,10 +70,10 @@ extern "C" {
  *
  *				The periodic interrupt timer is a simple callback interface.
  *				After installing the callback function pointer via #Timer_SetCallback,
- *				the timer can be started by setting interval via #Timer_SetInterval
- *				or #Timer_Start. From then, the callback is invoked periodically as
- *				the corresponding interval may specify. The timer is stopped via
- *				#Timer_Stop or by setting the interval to 0. The interval can be
+ *				the timer can be started by setting interval via #Timer_SetInterval.
+ *				From then, the callback is invoked periodically as the corresponding
+ *				interval may specify. The timer is stopped by setting the interval
+ *				to 0 using the #Timer_SetInterval function. The interval can be
  *				updated at any time by updating the interval via the #Timer_SetInterval
  *				function. To any of these functions, an abstract parameter pointer
  *				must be passed. This parameter is passed back to the callback any
@@ -84,11 +84,11 @@ extern "C" {
  *				at the same time. Therefore, the abstract parameter pointer is used
  *				to identify the corresponding callback interval. For example, there
  *				are two callbacks for two intervals, t1 and t2, required. The user
- *				can start two timers by calling the #Timer_Start method twice, but
- *				with an individual parameter pointer, ptr1 and ptr2, each:
+ *				can start two timers by calling the #Timer_SetInterval method twice,
+ *				but with an individual parameter pointer, ptr1 and ptr2, each:
  *				\code
- *					Timer_Start(100000, ptr1); // 10 ms callback w/ parameter ptr1
- *					Timer_Start(200000, ptr2); // 20 ms callback w/ parameter ptr1
+ *					Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1
+ *					Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1
  *				\endcode
  *
  *				Note that the implemented timer module must therefore support
@@ -215,50 +215,6 @@ status_t Timer_SetCallback(timer_cb_t f);
  *****************************************************************************/
 status_t Timer_SetInterval(uint32_t dt_microseconds, void *param);
 
-/*!***************************************************************************
- * @brief	Starts the timer for a specified callback parameter.
- *
- * @details	Sets the callback interval for the specified parameter and starts
- * 			the timer with a new interval. If there is already an interval with
- * 			the given parameter, the timer is restarted with the given interval.
- * 			Passing a interval of 0 disables the timer.
- *
- * 			Note that a microsecond granularity for the timer interrupt period is
- * 			not required. Usually a microseconds granularity is sufficient.
- * 			The required granularity depends on the targeted frame rate, e.g. in
- * 			case of more than 1 kHz measurement rate, a granularity of less than
- * 			a microsecond is required to achieve the given frame rate.
- *
- * @note	The implementation of this function is optional for the correct
- * 			execution of the API. If not implemented, a weak implementation
- * 			within the API will be used that disable the periodic timer callback
- * 			and thus the automatic starting of measurements from the background.
- *
- * @param	dt_microseconds The callback interval in microseconds.
- *
- * @param	param An abstract parameter to be passed to the callback. This is
- * 				  also the identifier of the given interval.
- *
- * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
- *****************************************************************************/
-status_t Timer_Start(uint32_t dt_microseconds, void *param);
-
-/*!***************************************************************************
- * @brief	Stops the timer for a specified callback parameter.
- *
- * @details	Stops a callback interval for the specified parameter.
- *
- * @note	The implementation of this function is optional for the correct
- * 			execution of the API. If not implemented, a weak implementation
- * 			within the API will be used that disable the periodic timer callback
- * 			and thus the automatic starting of measurements from the background.
- *
- * @param	param An abstract parameter that identifies the interval to be stopped.
- *
- * @return 	Returns the \link #status_t status\endlink (#STATUS_OK on success).
- *****************************************************************************/
-status_t Timer_Stop(void *param);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h
index 06f39681e59..ae7422ada59 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h
@@ -59,7 +59,11 @@
 #include <stdint.h>
 
 /*******************************************************************************
- * UQ6.2
+ *****   Unsigned 8-Bit Values   ***********************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** UQ6.2
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ6.2
@@ -70,9 +74,16 @@
  *****************************************************************************/
 typedef uint8_t uq6_2_t;
 
+/*! Maximum value of UQ6.2 number format. */
+#define UQ6_2_MAX ((uq6_2_t)UINT8_MAX)
+
+/*! The 1/one/unity in UQ6.2 number format. */
+#define UQ6_2_ONE ((uq6_2_t)(1U<<2U))
+
+
 
 /*******************************************************************************
- * UQ4.4
+ ***** UQ4.4
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ4.4
@@ -90,8 +101,9 @@ typedef uint8_t uq4_4_t;
 #define UQ4_4_ONE ((uq4_4_t)(1U<<4U))
 
 
+
 /*******************************************************************************
- * UQ2.6
+ ***** UQ2.6
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ2.6
@@ -102,11 +114,16 @@ typedef uint8_t uq4_4_t;
  *****************************************************************************/
 typedef uint8_t uq2_6_t;
 
+/*! Maximum value of UQ2.6 number format. */
+#define UQ2_6_MAX ((uq2_6_t)UINT8_MAX)
+
 /*! The 1/one/unity in UQ2.6 number format. */
 #define UQ2_6_ONE ((uq2_6_t)(1U<<6U))
 
+
+
 /*******************************************************************************
- * UQ1.7
+ ***** UQ1.7
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ1.7
@@ -117,20 +134,16 @@ typedef uint8_t uq2_6_t;
  *****************************************************************************/
 typedef uint8_t uq1_7_t;
 
-/*******************************************************************************
- * Q0.7
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q0.7
- * @details	An signed fixed point number format based on the 8-bit integer
- * 			type with 0 integer and 7 fractional bits.
- * 			- Range: -1 .. 0.9921875
- * 			- Granularity: 0.0078125
- *****************************************************************************/
-//typedef int8_t q0_7_t;
+/*! Maximum value of UQ1.7 number format. */
+#define UQ1_7_MAX ((uq1_7_t)UINT8_MAX)
+
+/*! The 1/one/unity in UQ1.7 number format. */
+#define UQ1_7_ONE ((uq1_7_t)(1U<<7U))
+
+
 
 /*******************************************************************************
- * UQ0.8
+ ***** UQ0.8
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ0.8
@@ -144,8 +157,62 @@ typedef uint8_t uq0_8_t;
 /*! Maximum value of UQ0.8 number format. */
 #define UQ0_8_MAX ((uq0_8_t)UINT8_MAX)
 
+
+
 /*******************************************************************************
- * UQ12.4
+ *****   Signed 8-Bit Values   *************************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** Q3.4
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q3.4
+ * @details	A signed fixed point number format based on the 8-bit signed
+ * 			integer type with 3 integer and 4 fractional bits.
+ * 			- Range: -8 ... 7.9375
+ * 			- Granularity: 0.0625
+ *****************************************************************************/
+typedef int8_t q3_4_t;
+
+/*! Minimum value of Q3.4 number format. */
+#define Q3_4_MIN ((q3_4_t)INT8_MIN)
+
+/*! Maximum value of Q3.4 number format. */
+#define Q3_4_MAX ((q3_4_t)INT8_MAX)
+
+/*! The 1/one/unity in UQ3.4 number format. */
+#define UQ3_4_ONE ((q3_4_t)(1 << 4))
+
+
+/*******************************************************************************
+ ***** Q1.6
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q1.6
+ * @details	A signed fixed point number format based on the 8-bit signed
+ * 			integer type with 1 integer and 6 fractional bits.
+ * 			- Range: -2 ... 1.984375
+ * 			- Granularity: 0.015625
+ *****************************************************************************/
+typedef int8_t q1_6_t;
+
+/*! Minimum value of Q1_6 number format. */
+#define Q1_6_MIN ((q1_6_t)INT8_MIN)
+
+/*! Maximum value of Q1.6 number format. */
+#define Q1_6_MAX ((q1_6_t)INT8_MAX)
+
+/*! The 1/one/unity in UQ1.6 number format. */
+#define UQ1_6_ONE ((q1_6_t)(1 << 4))
+
+
+/*******************************************************************************
+ *****   Unsigned 16-Bit Values   **********************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** UQ12.4
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ12.4
@@ -162,29 +229,10 @@ typedef uint16_t uq12_4_t;
 /*! The 1/one/unity in UQ12.4 number format. */
 #define UQ12_4_ONE ((uq12_4_t)(1U<<4U))
 
-/*******************************************************************************
- * Q11.4
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q11.4
- * @details	An signed fixed point number format based on the 16-bit signed
- * 			integer type with 11 integer and 4 fractional bits.
- * 			- Range: -2048 ... 2047.9375
- * 			- Granularity: 0.0625
- *****************************************************************************/
-typedef int16_t q11_4_t;
 
-/*! The 1/one/unity in UQ11.4 number format. */
-#define UQ11_4_ONE ((q11_4_t)(1 << 4))
-
-/*! Maximum value of Q11.4 number format. */
-#define Q11_4_MAX ((q11_4_t)INT16_MAX)
-
-/*! Minimum value of Q11.4 number format. */
-#define Q11_4_MIN ((q11_4_t)INT16_MIN)
 
 /*******************************************************************************
- * UQ10.6
+ ***** UQ10.6
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ10.6
@@ -201,8 +249,10 @@ typedef uint16_t uq10_6_t;
 /*! The 1/one/unity in UQ10.6 number format. */
 #define UQ10_6_ONE ((uq10_6_t)(1U << 6U))
 
+
+
 /*******************************************************************************
- * UQ1.15
+ ***** UQ1.15
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ1.15
@@ -219,58 +269,10 @@ typedef uint16_t uq1_15_t;
 /*! The 1/one/unity in UQ1.15 number format. */
 #define UQ1_15_ONE ((uq1_15_t)(1U << 15U))
 
-/*******************************************************************************
- * Q0.15
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q0.15
- * @details	An signed fixed point number format based on the 16-bit integer
- * 			type with 0 integer and 15 fractional bits.
- * 			- Range: -1 .. 0.999969482
- * 			- Granularity: 0.000030518
- *****************************************************************************/
-typedef int16_t q0_15_t;
-
-/*******************************************************************************
- * Q2.13
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q2.13
- * @details	An signed fixed point number format based on the 16-bit integer
- * 			type with 2 integer and 13 fractional bits.
- * 			- Range: -4 .. 3.99987793
- * 			- Granularity: 0.00012207
- *****************************************************************************/
-//typedef int16_t q2_13_t;
-
-/*******************************************************************************
- * Q13.2
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q13.2
- * @details	An signed fixed point number format based on the 16-bit integer
- * 			type with 13 integer and 2 fractional bits.
- * 			- Range: -8192 .. 8191.75
- * 			- Granularity: 0.25
- *****************************************************************************/
-//typedef int16_t q13_2_t;
 
 
 /*******************************************************************************
- * Q3.12
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q3.12
- * @details	An signed fixed point number format based on the 16-bit integer
- * 			type with 3 integer and 12 fractional bits.
- * 			- Range: -8 .. 7.99975586
- * 			- Granularity: 0.00024414
- *****************************************************************************/
-typedef int16_t q3_12_t;
-
-
-/*******************************************************************************
- * UQ0.16
+ ***** UQ0.16
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ0.16
@@ -284,11 +286,107 @@ typedef uint16_t uq0_16_t;
 /*! Maximum value of UQ0.16 number format. */
 #define UQ0_16_MAX ((uq0_16_t)UINT16_MAX)
 
-/*! The 1/one/unity in UQ0.16 number format. */
-#define UQ0_16_ONE ((uq0_16_t)(1U<<16U))
+
 
 /*******************************************************************************
- * UQ28.4
+ *****   Signed 16-Bit Values   ************************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** Q11.4
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q11.4
+ * @details	A signed fixed point number format based on the 16-bit signed
+ * 			integer type with 11 integer and 4 fractional bits.
+ * 			- Range: -2048 ... 2047.9375
+ * 			- Granularity: 0.0625
+ *****************************************************************************/
+typedef int16_t q11_4_t;
+
+/*! Minimum value of Q11.4 number format. */
+#define Q11_4_MIN ((q11_4_t)INT16_MIN)
+
+/*! Maximum value of Q11.4 number format. */
+#define Q11_4_MAX ((q11_4_t)INT16_MAX)
+
+/*! The 1/one/unity in Q11.4 number format. */
+#define UQ11_4_ONE ((q11_4_t)(1 << 4))
+
+
+
+/*******************************************************************************
+ ***** Q7.8
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q7.8
+ * @details	A signed fixed point number format based on the 16-bit signed
+ * 			integer type with 7 integer and 8 fractional bits.
+ * 			- Range: -128 .. 127.99609375
+ * 			- Granularity: 0.00390625
+ *****************************************************************************/
+typedef int16_t q7_8_t;
+
+/*! Minimum value of Q7.8 number format. */
+#define Q7_8_MIN ((q7_8_t)INT16_MIN)
+
+/*! Maximum value of Q7.8 number format. */
+#define Q7_8_MAX ((q7_8_t)INT16_MAX)
+
+/*! The 1/one/unity in Q7.8 number format. */
+#define Q7_8_ONE ((q7_8_t)(1 << 8))
+
+
+
+/*******************************************************************************
+ ***** Q3.12
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q3.12
+ * @details	A signed fixed point number format based on the 16-bit integer
+ * 			type with 3 integer and 12 fractional bits.
+ * 			- Range: -8 .. 7.99975586
+ * 			- Granularity: 0.00024414
+ *****************************************************************************/
+typedef int16_t q3_12_t;
+
+/*! Minimum value of Q3.12 number format. */
+#define Q3_12_MIN ((q3_12_t)INT16_MIN)
+
+/*! Maximum value of Q3.12 number format. */
+#define Q3_12_MAX ((q3_12_t)INT16_MAX)
+
+/*! The 1/one/unity in Q3.12 number format. */
+#define Q3_12_ONE ((q3_12_t)(1 << 12))
+
+
+
+/*******************************************************************************
+ ***** Q0.15
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q0.15
+ * @details	A signed fixed point number format based on the 16-bit integer
+ * 			type with 0 integer and 15 fractional bits.
+ * 			- Range: -1 .. 0.999969482
+ * 			- Granularity: 0.000030518
+ *****************************************************************************/
+typedef int16_t q0_15_t;
+
+/*! Minimum value of Q0.15 number format. */
+#define Q0_15_MIN ((q0_15_t)INT16_MIN)
+
+/*! Maximum value of Q0.15 number format. */
+#define Q0_15_MAX ((q0_15_t)INT16_MAX)
+
+
+
+/*******************************************************************************
+ *****   Unsigned 32-Bit Values   **********************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** UQ28.4
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ28.4
@@ -305,30 +403,10 @@ typedef uint32_t uq28_4_t;
 /*! The 1/one/unity in UQ28.4 number format. */
 #define UQ28_4_ONE ((uq28_4_t)(1U<<4U))
 
-/*******************************************************************************
- * Q27.4
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q27.4
- * @details	An signed fixed point number format based on the 32-bit signed
- * 			integer type with 27 integer and 4 fractional bits.
- * 			- Range: -134217728 ... 134217727.9375
- * 			- Granularity: 0.0625
- *****************************************************************************/
-typedef int32_t q27_4_t;
-
-/*! The 1/one/unity in UQ27.4 number format. */
-#define UQ27_4_ONE ((q27_4_t)(1 << 4))
-
-/*! Maximum value of Q27.4 number format. */
-#define Q27_4_MAX ((q27_4_t)INT32_MAX)
-
-/*! Minimum value of Q27.4 number format. */
-#define Q27_4_MIN ((q27_4_t)INT32_MIN)
 
 
 /*******************************************************************************
- * UQ16.16
+ ***** UQ16.16
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ16.16
@@ -339,39 +417,19 @@ typedef int32_t q27_4_t;
  *****************************************************************************/
 typedef uint32_t uq16_16_t;
 
-/*! The 1/one/unity in UQ16.16 number format. */
-#define UQ16_16_ONE ((uq16_16_t)(1U << 16U))
-
 /*! Maximum value of UQ16.16 number format. */
 #define UQ16_16_MAX ((uq16_16_t)UINT32_MAX)
 
+/*! The 1/one/unity in UQ16.16 number format. */
+#define UQ16_16_ONE ((uq16_16_t)(1U << 16U))
+
 /*! Euler's number, e, in UQ16.16 format. */
 #define UQ16_16_E (0x2B7E1U)
 
 
-/*******************************************************************************
- * Q15.16
- ******************************************************************************/
-/*!***************************************************************************
- * @brief	Signed fixed point number: Q15.16
- * @details	An signed fixed point number format based on the 32-bit integer
- * 			type with 15 integer and 16 fractional bits.
- * 			- Range: -32768 .. 32767.99998
- * 			- Granularity: 1.52588E-05
- *****************************************************************************/
-typedef int32_t q15_16_t;
-
-/*! The 1/one/unity in Q15.16 number format. */
-#define Q15_16_ONE ((q15_16_t)(1 << 16))
-
-/*! Maximum value of Q15.16 number format. */
-#define Q15_16_MAX ((q15_16_t)INT32_MAX)
-
-/*! Minimum value of Q15.16 number format. */
-#define Q15_16_MIN ((q15_16_t)INT32_MIN)
 
 /*******************************************************************************
- * UQ10.22
+ ***** UQ10.22
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Unsigned fixed point number: UQ10.22
@@ -382,26 +440,86 @@ typedef int32_t q15_16_t;
  *****************************************************************************/
 typedef uint32_t uq10_22_t;
 
+/*! Maximum value of UQ10.22 number format. */
+#define UQ10_22_MAX ((uq10_22_t)UINT32_MAX)
+
+/*! The 1/one/unity in UQ10.22 number format. */
+#define UQ10_22_ONE ((uq10_22_t)(1U << 22U))
+
+
+
 /*******************************************************************************
- * Q9.22
+ *****   Signed 32-Bit Values   ************************************************
+ ******************************************************************************/
+
+/*******************************************************************************
+ ***** Q27.4
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q27.4
+ * @details	A signed fixed point number format based on the 32-bit signed
+ * 			integer type with 27 integer and 4 fractional bits.
+ * 			- Range: -134217728 ... 134217727.9375
+ * 			- Granularity: 0.0625
+ *****************************************************************************/
+typedef int32_t q27_4_t;
+
+/*! Minimum value of Q27.4 number format. */
+#define Q27_4_MIN ((q27_4_t)INT32_MIN)
+
+/*! Maximum value of Q27.4 number format. */
+#define Q27_4_MAX ((q27_4_t)INT32_MAX)
+
+/*! The 1/one/unity in UQ27.4 number format. */
+#define UQ27_4_ONE ((q27_4_t)(1 << 4))
+
+
+
+/*******************************************************************************
+ ***** Q15.16
+ ******************************************************************************/
+/*!***************************************************************************
+ * @brief	Signed fixed point number: Q15.16
+ * @details	A signed fixed point number format based on the 32-bit integer
+ * 			type with 15 integer and 16 fractional bits.
+ * 			- Range: -32768 .. 32767.99998
+ * 			- Granularity: 1.52588E-05
+ *****************************************************************************/
+typedef int32_t q15_16_t;
+
+/*! Minimum value of Q15.16 number format. */
+#define Q15_16_MIN ((q15_16_t)INT32_MIN)
+
+/*! Maximum value of Q15.16 number format. */
+#define Q15_16_MAX ((q15_16_t)INT32_MAX)
+
+/*! The 1/one/unity in Q15.16 number format. */
+#define Q15_16_ONE ((q15_16_t)(1 << 16))
+
+
+
+/*******************************************************************************
+ ***** Q9.22
  ******************************************************************************/
 /*!***************************************************************************
  * @brief	Signed fixed point number: Q9.22
- * @details	An signed fixed point number format based on the 32-bit integer
+ * @details	A signed fixed point number format based on the 32-bit integer
  * 			type with 9 integer and 22 fractional bits.
  * 			- Range: -512 ... 511.9999998
  * 			- Granularity: 2.38418579101562E-07
  *****************************************************************************/
 typedef int32_t q9_22_t;
 
-/*! The 1/one/unity in Q9.22 number format. */
-#define Q9_22_ONE ((q9_22_t)(1 << 22))
+/*! Minimum value of Q9.22 number format. */
+#define Q9_22_MIN ((q9_22_t)INT32_MIN)
 
 /*! Maximum value of Q9.22 number format. */
 #define Q9_22_MAX ((q9_22_t)INT32_MAX)
 
-/*! Minimum value of Q9.22 number format. */
-#define Q9_22_MIN ((q9_22_t)INT32_MIN)
+/*! The 1/one/unity in Q9.22 number format. */
+#define Q9_22_ONE ((q9_22_t)(1 << 22))
+
+
 
 /*! @} */
 #endif /* FP_DEF_H */
diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h
index ed96f7337a4..9c5f930351d 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h
@@ -99,7 +99,7 @@ uint32_t Time_GetNowSec(void);
  * @param	t_elapsed Returns the elapsed time since t_start.
  * @param	t_start Start time point.
  *****************************************************************************/
-void Time_GetElapsed(ltc_t *t_elapsed, ltc_t *const t_start);
+void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start);
 
 /*!***************************************************************************
  * @brief	Obtains the elapsed microseconds since a given time point.
@@ -107,7 +107,7 @@ void Time_GetElapsed(ltc_t *t_elapsed, ltc_t *const t_start);
  * @param	t_start Start time point.
  * @return	Elapsed microseconds since t_start as uint32_t.
  *****************************************************************************/
-uint32_t Time_GetElapsedUSec(ltc_t *const t_start);
+uint32_t Time_GetElapsedUSec(ltc_t const *t_start);
 
 /*!***************************************************************************
  * @brief	Obtains the elapsed milliseconds since a given time point.
@@ -115,14 +115,14 @@ uint32_t Time_GetElapsedUSec(ltc_t *const t_start);
  * @param	t_start Start time point.
  * @return	Elapsed milliseconds since t_start as uint32_t.
  *****************************************************************************/
-uint32_t Time_GetElapsedMSec(ltc_t *const t_start);
+uint32_t Time_GetElapsedMSec(ltc_t const *t_start);
 
 /*!***************************************************************************
  * @brief	Obtains the elapsed seconds since a given time point.
  * @param	t_start Start time point.
  * @return	Elapsed seconds since t_start as uint32_t.
  *****************************************************************************/
-uint32_t Time_GetElapsedSec(ltc_t *const t_start);
+uint32_t Time_GetElapsedSec(ltc_t const *t_start);
 
 /*!***************************************************************************
  * @brief	Obtains the time difference between two given time points.
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 b99488bb4b4..c646ab31162 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 ae1c4f1d890..905b2fa66d3 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