From a1c05e74b5c071a4b3cf47e0255cd5ac7ec5ce89 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Dec 2020 14:52:29 +1100 Subject: [PATCH] AP_Vehicle: call init_safety after first loop has run this fixes a bug where servos can be driven to an out of range value if PWM output happens before first loop has completed thanks to Kris for reporting --- libraries/AP_Vehicle/AP_Vehicle.cpp | 12 ++++++++++++ libraries/AP_Vehicle/AP_Vehicle.h | 2 ++ 2 files changed, 14 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index c2fbb01e5c..cd89e4f2d4 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -148,6 +148,18 @@ void AP_Vehicle::loop() { scheduler.loop(); G_Dt = scheduler.get_loop_period_s(); + + if (!done_safety_init) { + /* + disable safety if requested. This is delayed till after the + first loop has run to ensure that all servos have received + an update for their initial values. Otherwise we may end up + briefly driving a servo to a position out of the configured + range which could damage hardware + */ + done_safety_init = true; + BoardConfig.init_safety(); + } } /* diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index fe61d57b18..62993d9726 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -307,6 +307,8 @@ private: uint32_t _last_flying_ms; // time when likely_flying last went true static AP_Vehicle *_singleton; + + bool done_safety_init; }; namespace AP {