浅谈APM系列 |
您所在的位置:网站首页 › copter和helicopter区别 › 浅谈APM系列 |
APM与PX4底层之间的关系
PX4创建任务之后,调用APM中的void Copter::setup()和void Copter::loop()函数 位置:X:\ardupilot\libraries\AP_HAL_PX4\HAL_PX4_Class.cpp 在HAL_PX4_Class.cpp文件中,有如下代码 static AP_HAL::HAL::Callbacks* g_callbacks; void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) g_callbacks = callbacks;在main_loop中调用了 g_callbacks->setup(); g_callbacks->loop();我们应该还记得APM中的 AP_HAL_MAIN_CALLBACKS(&copter);和 #pragma once #include "HAL.h" #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN #define AP_MAIN __EXPORT ArduPilot_main #endif #ifndef AP_MAIN #define AP_MAIN main #endif #define AP_HAL_MAIN() \ AP_HAL::HAL::FunCallbacks callbacks(setup, loop); \ extern "C" { \ int AP_MAIN(int argc, char* const argv[]); \ int AP_MAIN(int argc, char* const argv[]) { \ hal.run(argc, argv, &callbacks); \ return 0; \ } \ } #define AP_HAL_MAIN_CALLBACKS(CALLBACKS) extern "C" { \ int AP_MAIN(int argc, char* const argv[]); \ int AP_MAIN(int argc, char* const argv[]) { \ hal.run(argc, argv, CALLBACKS); \ return 0; \ } \ }其中的copter,是一个Copter的对象 Copter copter;看到这里你应该明白了,APM中的set函数和loop函数是怎么被调用的了 APM中的void Copter::setup()函数,由PX4调用 位置:X:\ardupilot\ArduCopter\ArduCopter.cpp void Copter::setup() { // Load the default values of variables listed in var_info[]s AP_Param::setup_sketch_defaults(); // setup storage layout for copter StorageManager::set_layout_copter(); init_ardupilot(); // initialise the main loop scheduler scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks)); // setup initial performance counters perf_info.reset(scheduler.get_loop_rate_hz()); fast_loopTimer = AP_HAL::micros(); }APM中的void Copter::loop()函数,同样是由PX4调用 位置:X:\ardupilot\ArduCopter\ArduCopter.cpp void Copter::loop() { // wait for an INS sample ins.wait_for_sample();//等待最新的传感器数据 uint32_t timer = micros(); // check loop time perf_info.check_loop_time(timer - fast_loopTimer); // used by PI Loops G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;//计算Dt,用于PID计算 fast_loopTimer = timer; // for mainloop failure monitoring mainLoop_count++; // Execute the fast loop // --------------------- fast_loop(); // tell the scheduler one tick has passed scheduler.tick(); // run all the tasks that are due to run. Note that we only // have to call this once per loop, as the tasks are scheduled // in multiples of the main loop tick. So if they don't run on // the first call to the scheduler they won't run on a later // call until scheduler.tick() is called again const uint32_t loop_us = scheduler.get_loop_period_us(); const uint32_t time_available = (timer + loop_us) - micros(); scheduler.run(time_available > loop_us ? 0u : time_available); }
明日复明日,明日何其多 后面将继续更新,APM的其他部分代码。希望能对你的学习和工作有所帮助。 请耐心等待。。。。。 |
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |
CopyRight 2018-2019 实验室设备网 版权所有 win10的实时保护怎么永久关闭 |