Skip to content
Snippets Groups Projects
Commit 2155951e authored by Randy Mackay's avatar Randy Mackay
Browse files

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
parent 1135ef36
No related branches found
No related tags found
No related merge requests found
...@@ -10,10 +10,6 @@ ...@@ -10,10 +10,6 @@
*/ */
static void update_auto(void) 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 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); float pitch = constrain_float(nav_status.pitch+g.pitch_trim, -90, 90);
update_pitch_servo(pitch); update_pitch_servo(pitch);
......
...@@ -89,6 +89,12 @@ static void update_tracking(void) ...@@ -89,6 +89,12 @@ static void update_tracking(void)
// update bearing and distance to vehicle // update bearing and distance to vehicle
update_bearing_and_distance(); 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) { switch (control_mode) {
case AUTO: case AUTO:
update_auto(); update_auto();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment