diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index cd29bb1fe8..cf1dcba942 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -40,7 +40,12 @@ void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, ui } // initialize the vehicle -void AP_Vehicle::init_vehicle() { +void AP_Vehicle::init_vehicle() +{ + if (init_done) { + return; + } + init_done = true; #if HAL_RUNCAM_ENABLED runcam.init(); #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 06b2510219..701e1443c0 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -112,6 +112,9 @@ public: void get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks); + // initialize the vehicle. Called from AP_BoardConfig + void init_vehicle(); + protected: // board specific config @@ -153,15 +156,13 @@ protected: AP_AHRS_DCM ahrs; #endif - // initialize the vehicle - void init_vehicle(); - static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Scheduler::Task scheduler_tasks[]; private: static AP_Vehicle *_singleton; + bool init_done; };