From 2155951e63b4b14ff29875c80f04d8227106633f Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Oct 2014 23:28:14 +0900 Subject: [PATCH] Tracker: move startup delay to update_tracking call This ensure the tracker does not move in any mode until after the startup delay has passed --- AntennaTracker/control_auto.pde | 4 ---- AntennaTracker/tracking.pde | 6 ++++++ 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/AntennaTracker/control_auto.pde b/AntennaTracker/control_auto.pde index 732050f35f..15827e00c5 100644 --- a/AntennaTracker/control_auto.pde +++ b/AntennaTracker/control_auto.pde @@ -10,10 +10,6 @@ */ static void update_auto(void) { - if (g.startup_delay > 0 && - hal.scheduler->millis() - start_time_ms < g.startup_delay*1000) { - return; - } float yaw = wrap_180_cd((nav_status.bearing+g.yaw_trim)*100) * 0.01f; float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90); update_pitch_servo(pitch); diff --git a/AntennaTracker/tracking.pde b/AntennaTracker/tracking.pde index f07d4a03d5..4187aba8c9 100644 --- a/AntennaTracker/tracking.pde +++ b/AntennaTracker/tracking.pde @@ -89,6 +89,12 @@ static void update_tracking(void) // update bearing and distance to vehicle update_bearing_and_distance(); + // do not perform any servo updates until startup delay has passed + if (g.startup_delay > 0 && + hal.scheduler->millis() - start_time_ms < g.startup_delay*1000) { + return; + } + switch (control_mode) { case AUTO: update_auto();