altos: Respond to telemetry rate changes immediately

[fw/altos] / src / core / ao_telemetry.c

diff --git a/src/core/ao_telemetry.c b/src/core/ao_telemetry.c
index 95e53917152c176c6dabc9067ab8ef659a7aa1e3..d909bea56178d47eb7ce45a5b86d6faa6dea0804 100644 (file)
--- a/src/core/ao_telemetry.c
+++ b/src/core/ao_telemetry.c
@@ -240,8 +240,11 @@ ao_telemetry(void)
 #endif
                        time += ao_telemetry_interval;
                        delay = time - ao_time();
-                       if (delay > 0)
-                               ao_delay(delay);
+                       if (delay > 0) {
+                               ao_alarm(delay);
+                               ao_sleep(&telemetry);
+                               ao_clear_alarm();
+                       }
                        else
                                time = ao_time();
                }