Set sensor mode if sensor is already setup

This helps when updating a module
TOPLLab · Oct 28, 2024 · 6faffce · 6faffce
1 parent 9fc795f
commit 6faffce
Showing 1 changed file with 7 additions and 1 deletion.
diff --git a/src/Primitives/zephyr.cpp b/src/Primitives/zephyr.cpp
@@ -886,7 +886,13 @@ def_prim(setup_uart_sensor, twoToNoneU32) {
         return 0;
     }
     uart_irq_rx_enable(uart_dev);
-    mode = arg0.uint32;
+    uint8_t new_mode = arg0.uint32;
+    if (new_mode != mode && data_mode) {
+        uart_poll_out(uart_dev, 0x43);
+        uart_poll_out(uart_dev, new_mode);
+        uart_poll_out(uart_dev, 0xff ^ 0x43 ^ new_mode);
+    }
+    mode = new_mode;
     pop_args(2);
     return true;
 }