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 {