diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 5c0b9cd386..d1ce301fa7 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -22,6 +22,35 @@ AP_Vehicle& vehicle = *AP_Vehicle::get_singleton(); extern AP_Vehicle& vehicle; #endif +/* + setup is called when the sketch starts + */ +void AP_Vehicle::setup() +{ + // load the default values of variables listed in var_info[] + AP_Param::setup_sketch_defaults(); + + // initialise serial port + serial_manager.init_console(); + + hal.console->printf("\n\nInit %s" + "\n\nFree RAM: %u\n", + AP::fwversion().fw_string, + (unsigned)hal.util->available_memory()); + + load_parameters(); + + // init_ardupilot is where the vehicle does most of its initialisation. + init_ardupilot(); + + // initialise the main loop scheduler + const AP_Scheduler::Task *tasks; + uint8_t task_count; + uint32_t log_bit; + get_scheduler_tasks(tasks, task_count, log_bit); + AP::scheduler().init(tasks, task_count, log_bit); +} + /* common scheduler table for fast CPUs - all common vehicle tasks should be listed here, along with how often they should be called (in hz) @@ -112,4 +141,3 @@ AP_Vehicle *vehicle() } }; - diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index c71cf2afe1..87f557b5c3 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -56,6 +56,15 @@ public: static AP_Vehicle *get_singleton(); + // setup() is called once during vehicle startup to initialise the + // vehicle object and the objects it contains. The + // AP_HAL_MAIN_CALLBACKS pragma creates a main(...) function + // referencing an object containing setup() and loop() functions. + // A vehicle is not expected to override setup(), but + // subclass-specific initialisation can be done in init_ardupilot + // which is called from setup(). + void setup(void) override final; + bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0; uint8_t virtual get_mode() const = 0; @@ -117,6 +126,8 @@ public: // initialize the vehicle. Called from AP_BoardConfig void init_vehicle(); + virtual void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) = 0; + /* set the "likely flying" flag. This is not guaranteed to be accurate, but is the vehicle codes best guess as to the whether @@ -151,6 +162,8 @@ public: protected: + virtual void init_ardupilot() = 0; + // board specific config AP_BoardConfig BoardConfig;