altos: Break out GPS speed resetting sequence

To set the GPS speed, we delay for 1/2 sec, change speed, then delay
for another 1/2 sec.

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

diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index d637a6027306b877cf38a0282f697fe8c1f68b9d..d2f67e6b6947b1f9cd4c8e1c87fc13a018eb23b7 100644 (file)
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -506,6 +506,13 @@ static __code uint8_t ao_gps_115200[] = {
        SKYTRAQ_MSG_3(5,0,5,0)  /* Set to 115200 baud */
 };
 
+static void
+ao_gps_set_speed_delay(uint8_t speed) {
+       ao_delay(AO_MS_TO_TICKS(500));
+       ao_gps_set_speed(speed);
+       ao_delay(AO_MS_TO_TICKS(500));
+}
+
 static void
 gps_update(void) __reentrant
 {
@@ -515,13 +522,9 @@ gps_update(void) __reentrant
        ao_timer_set_adc_interval(0);
 #endif
        ao_skytraq_sendstruct(ao_gps_115200);
-       ao_delay(AO_MS_TO_TICKS(500));
-       ao_gps_set_speed(AO_SERIAL_SPEED_4800);
-       ao_delay(AO_MS_TO_TICKS(500));
+       ao_gps_set_speed_delay(AO_SERIAL_SPEED_4800);
        ao_skytraq_sendstruct(ao_gps_115200);
-       ao_delay(AO_MS_TO_TICKS(500));
-       ao_gps_set_speed(AO_SERIAL_SPEED_115200);
-       ao_delay(AO_MS_TO_TICKS(500));
+       ao_gps_set_speed_delay(AO_SERIAL_SPEED_115200);
 
        /* It's a binary protocol; abandon attempts to escape */
        for (;;)