altos: Respond to telemetry rate changes immediately

Instead of waiting for the previous telemetry interval to expire,
immediately switch to the new telemetry rate. This will provide
more telemetry data early in the boost.

Signed-off-by: Keith Packard <keithp@keithp.com>

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();
                }