From 79f6b84190c3d336dac737c1eec7a1ec781d5eb8 Mon Sep 17 00:00:00 2001
From: Jacob Dahl <dahl.jakejacob@gmail.com>
Date: Wed, 9 Jul 2025 23:23:03 -0800
Subject: [PATCH] add check for DFMMode after setting it

---
 .../distance_sensor/broadcom/afbrs50/AFBRS50.cpp    | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
index 3e272f10b9..53c681d435 100644
--- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
+++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp
@@ -370,6 +370,19 @@ status_t AFBRS50::setRateAndDfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
 		return status;
 	}
 
+	argus_dfm_mode_t result_mode;
+	status = Argus_GetConfigurationDFMMode(_hnd, &result_mode);
+
+	if (status != STATUS_OK) {
+		PX4_ERR("Argus_GetConfigurationDFMMode status not okay: %i", (int)status);
+		return status;
+	}
+
+	if (result_mode != dfm_mode) {
+		PX4_ERR("Argus_SetConfigurationDFMMode failed: %i", (int)status);
+		return status;
+	}
+
 	status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz));
 
 	if (status != STATUS_OK) {