From e24fef70f63e2d221b3ee0b775ca2afe38255019 Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Sun, 16 Apr 2017 20:14:23 -0400 Subject: [PATCH] Sub: Workaround for more graceful servo mount initialization --- ArduSub/ArduSub.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index 803515e540..02751cf00f 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -220,7 +220,10 @@ void Sub::fifty_hz_loop() // Update servo output RC_Channels::set_pwm_all(); - SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f); + // wait for outputs to initialize: TODO find a better way to do this + if (millis() > 10000) { + SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f); + } SRV_Channels::output_ch_all(); }