Ardupilot chibios主函式學習（1）
阿新 • • 發佈：2018-11-11
目錄
文章目錄
摘要
本節主要記錄自己學習Ardupilot的 chibios程式碼過程，這裡是從應用層main()函式實現開始講解。後續將會講解飛控上電執行的第一條指令，並且怎麼運轉到main()函式的過程，歡迎批評指正。
0序言
apm飛控的韌體ardupilot，採用Nuttx作業系統，最新的程式碼已經採用Chibios，本人已經驗證，效果不錯！
習慣了MDK-keil的朋友，喜歡上來直接找main()函式，記得我剛開始學STM32時，也是如此，後續有很多疑惑(微控制器一上電怎麼執行的？第一條程式碼從哪裡執行？我點選keil上的編譯按鈕就可以直接編譯自己寫的程式碼，upload就可以直接下載到微控制器,為什麼，這是什麼鬼？？？
記得自己剛開始學習ardupilot韌體也是一臉懵逼，現在回想起來一把辛酸淚，自己曾在微信豪言壯志說：“勵志做一代飛控大神”，說出來容易，需要付出很多，就是抱著不服輸的精神，為啥別人可以，而我切做不到，相信自己一定可以的，在很多人的幫助，和自學下，總算跨進ardupilot的大門！！！再次謝謝所有幫助我的大神們！！！
第一部分chibios主函式
在分析chibios之前，，先看一張FreeRTOS的圖片
對比分析你會有新的發現！！！
(1)主回撥函式入口
AP_HAL_MAIN_CALLBACKS(&copter);
(2)主回撥函式入口
#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); \ //這裡要關心這個函式，我們使用chibios,所以會選擇對應的hal
return 0; \
} \
}
需要注意的是這個函式：AP_HAL_MAIN（）
#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; \
} \
}
最新的程式碼已經不用這個，可以登出掉，編譯不會有問題，這個函式是用在每個sketch 最底部。
(3)主回撥函式入口
//ChibiOS HAL初始化，這也初始化配置的裝置驅動程式。並執行特定的板層初始化：核心初始化，main()函式變成一個執行緒，RTOS是執行的。
void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{
/*
* System initializations.---系統初始化
* - ChibiOS HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
#ifdef HAL_USB_PRODUCT_ID
setup_usb_strings(); //初始化usb
#endif
#ifdef HAL_STDOUT_SERIAL
//STDOUT Initialistion
SerialConfig stdoutcfg =
{
HAL_STDOUT_BAUDRATE,
0,
USART_CR2_STOP1_BITS,
0
};
sdStart((SerialDriver*)&HAL_STDOUT_SERIAL, &stdoutcfg);
#endif
assert(callbacks);
g_callbacks = callbacks;
//主執行緒建立
void *main_thread_wa = hal.util->malloc_type(THD_WORKING_AREA_SIZE(APM_MAIN_THREAD_STACK_SIZE), AP_HAL::Util::MEM_FAST);
chThdCreateStatic(main_thread_wa,
APM_MAIN_THREAD_STACK_SIZE,
APM_MAIN_PRIORITY, /* Initial priority. */ //執行緒的優先順序
main_loop, /* Thread function. */ //函式體，這裡是我們最關心的函式
nullptr); /* Thread parameter. */
chThdExit(0);
}
run函式是不是和freeRTOS很像！！！就是建立了main_loop執行緒函式，該執行緒一定會建立很多工？？！帶著這個問題看程式碼：初始化—建立任務—
(4)主回撥函式入口
static THD_FUNCTION(main_loop,arg)
{
daemon_task = chThdGetSelfX();
#ifdef HAL_I2C_CLEAR_BUS
// Clear all I2C Buses. This can be needed on some boards which
// can get a stuck I2C peripheral on boot
ChibiOS::I2CBus::clear_all();
#endif
ChibiOS::Shared_DMA::init(); //DMA配置
peripheral_power_enable(); //電源使能
hal.uartA->begin(115200); //usb波特率設定115200
#ifdef HAL_SPI_CHECK_CLOCK_FREQ
//SPI時鐘頻率的可選測試---- optional test of SPI clock frequencies
ChibiOS::SPIDevice::test_clock_freq();
#endif
//初始化SD卡和檔案系統---Setup SD Card and Initialise FATFS bindings
sdcard_init();
hal.uartB->begin(38400); //初始化GPS波特率38400
hal.uartC->begin(57600); //初始化數傳波特率
hal.analogin->init(); //模擬埠初始化
hal.scheduler->init(); //找到了？??任務排程初始化，這個建立了很多初始化任務，將會在下面進行分析
/*
run setup() at low priority to ensure CLI doesn't hang the
system, and to allow initial sensor read loops to run
執行初始化低優先順序任務，確保CLI不影響系統，同時允許感測器讀取執行
*/
hal_chibios_set_priority(APM_STARTUP_PRIORITY); //初始化優先順序
schedulerInstance.hal_initialized();
g_callbacks->setup(); //初始化函式
hal.scheduler->system_initialized();
thread_running = true;
chRegSetThreadName(SKETCHNAME);
/*
switch to high priority for main loop
*/
chThdSetPriority(APM_MAIN_PRIORITY);
while (true)
{
g_callbacks->loop(); //這個是主要的執行緒
/*
give up 250 microseconds of time if the INS loop hasn't
called delay_microseconds_boost(), to ensure low priority
drivers get a chance to run. Calling
delay_microseconds_boost() means we have already given up
time from the main loop, so we don't need to do it again
here
*/
if (!schedulerInstance.check_called_boost()) //放棄250us的延遲
{
hal.scheduler->delay_microseconds(250);
}
}
thread_running = false;
}
1.函式hal.scheduler->init()分析
主要實現：
定時器訊號量
io訊號量建立
定時器執行緒建立
遙控器執行緒建立
IO執行緒建立
儲存執行緒建立
void Scheduler::init()
{
chBSemObjectInit(&_timer_semaphore, false); //定時器訊號量
chBSemObjectInit(&_io_semaphore, false); //io訊號量建立
//初始化定時器執行緒，這個任務執行週期是1Khz----setup the timer thread - this will call tasks at 1kHz
_timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
sizeof(_timer_thread_wa),
APM_TIMER_PRIORITY, /* Initial priority. */ //#define APM_TIMER_PRIORITY 181
_timer_thread, /* Thread function. */
this); /* Thread parameter. */
//設定遙控器輸入RCIN執行緒-這將呼叫任務在1kHz--- setup the RCIN thread - this will call tasks at 1kHz
_rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
sizeof(_rcin_thread_wa),
APM_RCIN_PRIORITY, /* Initial priority. *///#define APM_RCIN_PRIORITY 177
_rcin_thread, /* Thread function. */
this); /* Thread parameter. */
// O執行緒以較低優先順序執行----the IO thread runs at lower priority
_io_thread_ctx = chThdCreateStatic(_io_thread_wa,
sizeof(_io_thread_wa),
APM_IO_PRIORITY, /* Initial priority. */ //#define APM_IO_PRIORITY 58
_io_thread, /* Thread function. */
this); /* Thread parameter. */
//the storage thread runs at just above IO priority---- the storage thread runs at just above IO priority
_storage_thread_ctx = chThdCreateStatic(_storage_thread_wa,
sizeof(_storage_thread_wa),
APM_STORAGE_PRIORITY, /* Initial priority. *///#define APM_STORAGE_PRIORITY 59
_storage_thread, /* Thread function. */
this); /* Thread parameter. */
}
1.定時器訊號量建立
chBSemObjectInit(&_timer_semaphore, false); //定時器訊號量
//訊號量函式可以先不用關心具體實現
static inline void chBSemObjectInit(binary_semaphore_t *bsp, bool taken)
{
chSemObjectInit(&bsp->sem, taken ? (cnt_t)0 : (cnt_t)1);
}
void chSemObjectInit(semaphore_t *sp, cnt_t n)
{
chDbgCheck((sp != NULL) && (n >= (cnt_t)0));
queue_init(&sp->queue);
sp->cnt = n;
}
2.io訊號量建立
chBSemObjectInit(&_io_semaphore, false);
//定義
binary_semaphore_t _timer_semaphore;
binary_semaphore_t _io_semaphore;
typedef struct ch_binary_semaphore {
semaphore_t sem;
} binary_semaphore_t;
3.定時器執行緒建立
_timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
sizeof(_timer_thread_wa),
APM_TIMER_PRIORITY, /* Initial priority. */
_timer_thread, /* Thread function. */
this); /* Thread parameter. */
這裡主要看執行緒實現函式
void Scheduler::_timer_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_timer");
while (!sched->_hal_initialized)
{
sched->delay_microseconds(1000);
}
while (true)
{
sched->delay_microseconds(1000);
//執行註冊計時器--- run registered timers
sched->_run_timers();
//處理任何未決的RC輸出請求---- process any pending RC output requests
hal.rcout->timer_tick();
}
}
void Scheduler::_run_timers()
{
if (_in_timer_proc)
{
return;
}
_in_timer_proc = true;
int num_procs = 0;
chBSemWait(&_timer_semaphore);
num_procs = _num_timer_procs;
chBSemSignal(&_timer_semaphore);
//現在呼叫基於計時器的驅動程式----- now call the timer based drivers
for (int i = 0; i < num_procs; i++)
{
if (_timer_proc[i])
{
_timer_proc[i]();
}
}
// and the failsafe, if one is setup
if (_failsafe != nullptr) {
_failsafe();
}
#if HAL_USE_ADC == TRUE
// process analog input
((AnalogIn *)hal.analogin)->_timer_tick();
#endif
_in_timer_proc = false;
}
執行緒建立函式
4.遙控器執行緒建立
// setup the RCIN thread - this will call tasks at 1kHz
_rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
sizeof(_rcin_thread_wa),
APM_RCIN_PRIORITY, /* Initial priority. */
_rcin_thread, /* Thread function. */
this); /* Thread parameter. */
void Scheduler::_rcin_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_rcin");
while (!sched->_hal_initialized)
{
sched->delay_microseconds(20000);
}
while (true)
{
sched->delay_microseconds(2500);
((RCInput *)hal.rcin)->_timer_tick();
}
}
void RCInput::_timer_tick(void)
{
if (!_init) {
return;
}
#if HAL_USE_ICU == TRUE || HAL_USE_EICU == TRUE
uint32_t width_s0, width_s1;
while(sig_reader.read(width_s0, width_s1)) {
rcin_prot.process_pulse(width_s0, width_s1);
}
if (rcin_prot.new_input()) {
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
_rcin_timestamp_last_signal = AP_HAL::micros();
_num_channels = rcin_prot.num_channels();
for (uint8_t i=0; i<_num_channels; i++) {
_rc_values[i] = rcin_prot.read(i);
}
rcin_mutex.give();
}
#endif
#if HAL_RCINPUT_WITH_AP_RADIO
if (radio && radio->last_recv_us() != last_radio_us) {
last_radio_us = radio->last_recv_us();
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
_rcin_timestamp_last_signal = last_radio_us;
_num_channels = radio->num_channels();
for (uint8_t i=0; i<_num_channels; i++) {
_rc_values[i] = radio->read(i);
}
rcin_mutex.give();
}
#endif
#if HAL_WITH_IO_MCU
rcin_mutex.take(HAL_SEMAPHORE_BLOCK_FOREVER);
if (AP_BoardConfig::io_enabled() &&
iomcu.check_rcinput(last_iomcu_us, _num_channels, _rc_values, RC_INPUT_MAX_CHANNELS)) {
_rcin_timestamp_last_signal = last_iomcu_us;
}
rcin_mutex.give();
#endif
// note, we rely on the vehicle code checking new_input()
// and a timeout for the last valid input to handle failsafe
}
5.IO執行緒建立
// the IO thread runs at lower priority
_io_thread_ctx = chThdCreateStatic(_io_thread_wa,
sizeof(_io_thread_wa),
APM_IO_PRIORITY, /* Initial priority. */
_io_thread, /* Thread function. */
this); /* Thread parameter. */
void Scheduler::_io_thread(void* arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_io");
while (!sched->_hal_initialized) {
sched->delay_microseconds(1000);
}
while (true) {
sched->delay_microseconds(1000);
// 執行註冊IO程序---run registered IO processes
sched->_run_io();
}
}
void Scheduler::_run_io(void)
{
if (_in_io_proc)
{
return;
}
_in_io_proc = true;
int num_procs = 0;
chBSemWait(&_io_semaphore);
num_procs = _num_io_procs;
chBSemSignal(&_io_semaphore);
// now call the IO based drivers
for (int i = 0; i < num_procs; i++)
{
if (_io_proc[i])
{
_io_proc[i]();
}
}
_in_io_proc = false;
}
6.儲存執行緒建立
// the storage thread runs at just above IO priority
_storage_thread_ctx = chThdCreateStatic(_storage_thread_wa,
sizeof(_storage_thread_wa),
APM_STORAGE_PRIORITY, /* Initial priority. */
_storage_thread, /* Thread function. */
this); /* Thread parameter. */
void Scheduler::_storage_thread(void* arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_storage");
while (!sched->_hal_initialized) {
sched->delay_microseconds(10000);
}
while (true) {
sched->delay_microseconds(10000);
// process any pending storage writes
hal.storage->_timer_tick();
}
}
void Storage::_timer_tick(void)
{
if (!_initialised)
{
return;
}
if (_dirty_mask.empty())
{
_last_empty_ms = AP_HAL::millis();
return;
}
// write out the first dirty line. We don't write more
// than one to keep the latency of this call to a minimum
uint16_t i;
for (i=0; i<CH_STORAGE_NUM_LINES; i++)
{
if (_dirty_mask.get(i))
{
break;
}
}
if (i == CH_STORAGE_NUM_LINES)
{
// this shouldn't be possible
return;
}
#if HAL_WITH_RAMTRON
if (using_fram)
{
if (fram.write(CH_STORAGE_LINE_SIZE*i, &_buffer[CH_STORAGE_LINE_SIZE*i], CH_STORAGE_LINE_SIZE))
{
_dirty_mask.clear(i);
}
return;
}
#endif
#ifdef USE_POSIX
if (using_filesystem && log_fd != -1) {
uint32_t offset = CH_STORAGE_LINE_SIZE*i;
if (lseek(log_fd, offset, SEEK_SET) != offset) {
return;
}
if (write(log_fd, &_buffer[offset], CH_STORAGE_LINE_SIZE) != CH_STORAGE_LINE_SIZE) {
return;
}
if (fsync(log_fd) != 0) {
return;
}
_dirty_mask.clear(i);
return;
}
#endif
#ifdef STORAGE_FLASH_PAGE
// save to storage backend
_flash_write(i);
#endif
}
2.函式g_callbacks->setup()分析
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();
//初始化整個主loop任務排程-------initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
通過scheduler_task[0]傳入陣列任務表
陣列表總共有58個需要執行的任務：可以通過設定ENABLE、DISABLED進行啟動或者禁用任務
有三組引數需要注意
- 1.任務函式：任務執行函式體
- 2.任務執行的更新頻率：保證任務多久執行一次
- 3.任務執行一次花費cpu的時間：保證該任務執行一次花費的CPU時間
const AP_Scheduler::Task Copter::scheduler_tasks[] =
{
SCHED_TASK(rc_loop, 100, 130), //遙控器資料處理函式
SCHED_TASK(throttle_loop, 50, 75), //油門迴圈函式
SCHED_TASK(update_GPS, 50, 200), //更新GPS資訊
#if OPTFLOW == ENABLED
SCHED_TASK(update_optical_flow, 200, 160), //更新光流資訊
#endif
SCHED_TASK(update_batt_compass, 10, 120), //更新電池和羅盤資訊
SCHED_TASK(read_aux_all, 10, 50), //讀取外部開關資料
SCHED_TASK(arm_motors_check, 10, 50), //電機解鎖檢查
#if TOY_MODE_ENABLED == ENABLED
SCHED_TASK_CLASS(ToyMode, &copter.g2.toy_mode, update, 10, 50),
#endif
SCHED_TASK(auto_disarm_check, 10, 50), //自動上鎖檢查
SCHED_TASK(auto_trim, 10, 75),
#if RANGEFINDER_ENABLED == ENABLED
SCHED_TASK(read_rangefinder, 20, 100), //讀取測距儀資料
#endif
#if PROXIMITY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 100, 50), //更新避障資料
#endif
#if BEACON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
SCHED_TASK(update_visual_odom, 400, 50),
#endif
SCHED_TASK(update_altitude, 10, 100), //更新定高資料
SCHED_TASK(update_mylog, 10, 100), //更新測試引數
SCHED_TASK(run_nav_updates, 50, 100), //更新導航 資料
SCHED_TASK(update_throttle_hover,100, 90), //更新油門
#if MODE_SMARTRTL_ENABLED == ENABLED
SCHED_TASK_CLASS(Copter::ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100),
#endif
#if SPRAYER_ENABLED == ENABLED
SCHED_TASK_CLASS(AC_Sprayer, &copter.sprayer, update, 3, 90),
#endif
SCHED_TASK(three_hz_loop, 3, 75),
SCHED_TASK_CLASS(AP_ServoRelayEvents, &copter.ServoRelayEvents, update_events, 50, 75),
SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90),
#if PRECISION_LANDING == ENABLED
SCHED_TASK(update_precland, 400, 50),
#endif
#if FRAME_CONFIG == HELI_FRAME
SCHED_TASK(check_dynamic_flight, 50, 75),
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(fourhundred_hz_logging,400, 50),
#endif
SCHED_TASK_CLASS(AP_Notify, &copter.notify, update, 50, 90), //led通知更新
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(gpsglitch_check, 10, 50),
SCHED_TASK(landinggear_update, 10, 75),
SCHED_TASK(lost_vehicle_check, 10, 50),
SCHED_TASK(gcs_update, 400, 180), //更新資料---2.5ms
SCHED_TASK(gcs_send_heartbeat, 1, 110), //傳送心跳包---1s
SCHED_TASK(gcs_send_deferred, 50, 550), //傳送輸入---20ms
SCHED_TASK(gcs_data_stream_send, 50, 550), //傳送資料流--20ms
#if MOUNT == ENABLED
SCHED_TASK_CLASS(AP_Mount, &copter.camera_mount, update, 50, 75), //掛載資訊
#endif
#if CAMERA == ENABLED
SCHED_TASK_CLASS(AP_Camera, &copter.camera, update_trigger, 50, 75), //相機資訊
#endif
#if LOGGING_ENABLED == ENABLED
SCHED_TASK(ten_hz_logging_loop, 10, 350),
SCHED_TASK(twentyfive_hz_logging, 25, 110),
SCHED_TASK_CLASS(DataFlash_Class, &copter.DataFlash, periodic_tasks, 400, 300),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50), //慣性感測器
SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75),
#if RPM_ENABLED == ENABLED
SCHED_TASK(rpm_update, 10, 200),
#endif
SCHED_TASK(compass_cal_update, 100, 100),
SCHED_TASK(accel_cal_update, 10, 100), //加速度計算更新
SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100),
#if ADSB_ENABLED == ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
#if ADVANCED_FAILSAFE == ENABLED
SCHED_TASK(afs_fs_check, 10, 100),
#endif
#if AC_TERRAIN == ENABLED
SCHED_TASK(terrain_update, 10, 100),
#endif
#if GRIPPER_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Gripper, &copter.g2.gripper, update, 10, 75),
#endif
#if WINCH_ENABLED == ENABLED
SCHED_TASK(winch_update, 10, 50),
#endif
#ifdef USERHOOK_FASTLOOP
SCHED_TASK(userhook_FastLoop, 100, 75),
#endif
#ifdef USERHOOK_50HZLOOP
SCHED_TASK(userhook_50Hz, 50, 75),
#endif
#ifdef USERHOOK_MEDIUMLOOP
SCHED_TASK(userhook_MediumLoop, 10, 75),
#endif
#ifdef USERHOOK_SLOWLOOP
SCHED_TASK(userhook_SlowLoop, 3.3, 75),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
SCHED_TASK(userhook_SuperSlowLoop, 1, 75),
#endif
SCHED_TASK_CLASS(AP_Button, &copter.g2.button, update, 5, 100),
#if STATS_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Stats, &copter.g2.stats, update, 1, 100),
#endif
#if OSD_ENABLED == ENABLED
SCHED_TASK(publish_osd_info, 1, 10),
#endif
};
那麼這個任務表，有那麼多工怎麼被執行？
3.函式g_callbacks->loop()分析
void Copter::loop()
{
scheduler.loop();
G_Dt = scheduler.get_last_loop_time_s();
}
void AP_Scheduler::loop()
{
// 等待INS資訊----wait for an INS sample
AP::ins().wait_for_sample();
const uint32_t sample_time_us = AP_HAL::micros();
//記錄該函式執行時間
if (_loop_timer_start_us == 0)
{
_loop_timer_start_us = sample_time_us;
_last_loop_time_s = get_loop_period_s();
} else
{
_last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
}
// Execute the fast loop
// ---------------------
if (_fastloop_fn)
{
_fastloop_fn();//注意這個函式怎麼執行到fast_loop()函式
}
// tell the scheduler one tick has passed
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 = get_loop_period_us();
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros();
run(time_available > loop_us ? 0u : time_available); //重點分析的函式
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// move result of AP_HAL::micros() forward:
hal.scheduler->delay_microseconds(1);
#endif
// check loop time
perf_info.check_loop_time(sample_time_us - _loop_timer_start_us);
_loop_timer_start_us = sample_time_us;
}
這裡主要分析兩個函式：
(1) _fastloop_fn();//注意這個函式怎麼執行到fast_loop()函式
(2) run(time_available > loop_us ? 0u : time_available);
1） _fastloop_fn();
思考第一個問題：函式_fastloop_fn（）怎麼執行到fast_loop()函式這裡？
只把主要的程式碼寫了出來
class AP_Scheduler
{
public:
...此處省略....
FUNCTOR_TYPEDEF(scheduler_fastloop_fn_t, void);
AP_Scheduler(scheduler_fastloop_fn_t fastloop_fn = nullptr);
struct Task
{
task_fn_t function;
const char *name;
float rate_hz;
uint16_t max_time_micros;
};
private:
// function that is called before anything in the scheduler table:
scheduler_fastloop_fn_t _fastloop_fn;
};
class Copter : public AP_HAL::HAL::Callbacks {
public:
Copter(void);
// HAL::Callbacks implementation.
void setup() override;
void loop() override;
private:
// main loop scheduler
AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&Copter::fast_loop, void)};
void fast_loop();
}；
這裡採用了函式指標的使用，把_fastloop_fn()函式與fast_loop（）函式指向同一個地址
void Copter::fast_loop()
{
//hal.uartG->printf("NNN0\r\n"); //loop_us=2500,這個單位是us
// update INS immediately to get current gyro data populated
ins.update();
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
// send outputs to the motors library immediately
motors_output();
// run EKF state estimator (expensive)
// --------------------
read_AHRS();
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME
// Inertial Nav
// --------------------
read_inertia();
// check if ekf has reset target heading or position
check_ekf_reset();
//執行姿態控制器------run the attitude controllers
update_flight_mode();
// update home from EKF if necessary
update_home_from_EKF();
// check if we've landed or crashed
update_land_and_crash_detectors();
#if MOUNT == ENABLED
// camera mount's fast update
camera_mount.update_fast();
#endif
// log sensor health
if (should_log(MASK_LOG_ANY))
{
Log_Sensor_Health();
}
// hal.uartG->printf("NNN1\r\n"); //loop_us=2500,這個單位是us
}
(2) run(time_available > loop_us ? 0u : time_available)
思考第二個問題：怎麼保證fast_loop（）函式與任務陣列表scheduler_tasks[]正常運轉？
// tell the scheduler one tick has passed
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 = get_loop_period_us();
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros();
run(time_available > loop_us ? 0u : time_available);
繼續看loop函式，這裡只貼出主要的部分
void AP_Scheduler::loop()
{
// wait for an INS sample
AP::ins().wait_for_sample(); //等待INS資料
const uint32_t sample_time_us = AP_HAL::micros(); //獲取系統取樣時間
if (_loop_timer_start_us == 0)
{
_loop_timer_start_us = sample_time_us;
_last_loop_time_s = get_loop_period_s();
} else
{
_last_loop_time_s = (sample_time_us - _loop_timer_start_us) * 1.0e-6;
}
//執行快速迴圈函式----Execute the fast loop
// ---------------------
if (_fastloop_fn)
{
_fastloop_fn();
}
//高速度排程器，一個任務節拍已經執行------tell the scheduler one tick has passed
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 = get_loop_period_us(); //這個是主迴圈時間，400HZ，
const uint32_t time_available = (sample_time_us + loop_us) - AP_HAL::micros(); //獲取執行完fast_loop()函式後，剩餘多少時間給陣列表任務使用
run(time_available > loop_us ? 0u : time_available); //執行函式，上面計算剩餘的時間都留給任務表中的任務去用。
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// move result of AP_HAL::micros() forward:
hal.scheduler->delay_microseconds(1);
#endif
// check loop time
perf_info.check_loop_time(sample_time_us - _loop_timer_start_us);
_loop_timer_start_us = sample_time_us;
}
void AP_Scheduler::run(uint32_t time_available)
{
// hal.uartG->printf("AP_Scheduler Run\r\n");
uint32_t run_started_usec = AP_HAL::micros(); //執行開始時間
uint32_t now = run_started_usec; //記錄這個值，開始執行時間
//除錯
if (_debug > 1 && _perf_counters == nullptr)
{
_perf_counters = new AP_HAL::Util::perf_counter_t[_num_tasks];
if (_perf_counters != nullptr)
{
for (uint8_t i=0; i<_num_tasks; i++)
{
_perf_counters[i] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, _tasks[i].name); //效能計數
}
}
}
//通過for迴圈，在允許的時間範圍內，儘可能的多執行任務表中的任務，這裡舉個例子，假如我們還剩1.5ms的時間給剩餘的任務表使用；
for (uint8_t i=0; i<_num_tasks; i++)
{
hal.uartG->printf("i=%d\r\n",i); //
uint16_t dt = _tick_counter - _last_run[i]; //_last_run[i]:執行一個loop，記錄的任務表中每個任務被被執行的次數,因此dt表示上次執行的那個任務到現在這裡的圈數
// hal.uartG->printf("_tick_counter=%d\r\n",_tick_counter); //time_available=2212..
// hal.uartG->printf("_last_run[i]=%d\r\n",_last_run[i]); //time_available=2212..
//_loop_rate_hz 是迴圈的頻率，這裡是400Hz，_tasks[i].rate_hz是任務表中的某個任務的頻率，假如是20Hz，那麼interval_ticks =20，
//那就應該是運行了20圈後,執行該任務。
uint16_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz; //這個值表示執行某個任務需要的圈數
if (interval_ticks < 1) //如果這個值小於1，就預設是1，一般小於很少
{
interval_ticks = 1;
}
if (dt < interval_ticks) //如果dt<interval_ticks將會結束繼續允許該任務
{
//這個任務不會在被執行------- this task is not yet scheduled to run again
continue;
}
// this task is due to run. Do we have enough time to run it?
_task_time_allowed = _tasks[i].max_time_micros;
if (dt >= interval_ticks*2)
{
// we've slipped a whole run of this task!
debug(2, "Scheduler slip task[%u-%s] (%u/%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)dt,
(unsigned)interval_ticks,
(unsigned)_task_time_allowed);
}
//這個時間太大是不行，不會執行
if (_task_time_allowed > time_available)
{
// not enough time to run this task. Continue loop -
// maybe another task will fit into time remaining
continue;
}
// run it
_task_time_started = now;
current_task = i;
if (_debug > 1 && _perf_counters && _perf_counters[i])
{
hal.util->perf_begin(_perf_counters[i]);
}
// hal.uartG->printf("BBB\r\n"); //time_available=2212..
//執行任務函式
_tasks[i].function();
// hal.uartG->printf("BBB12\r\n"); //time_available=2212..
if (_debug > 1 && _perf_counters && _perf_counters[i])
{
hal.util->perf_end(_perf_counters[i]);
}
current_task = -1;
// record the tick counter when we ran. This drives
// when we next run the event
_last_run[i] = _tick_counter;
// work out how long the event actually took
now = AP_HAL::micros();
uint32_t time_taken = now - _task_time_started;
if (time_taken > _task_time_allowed)
{
// the event overran!
debug(3, "Scheduler overrun task[%u-%s] (%u/%u)\n",
(unsigned)i,
_tasks[i].name,
(unsigned)time_taken,
(unsigned)_task_time_allowed);
}
if (time_taken >= time_available)
{
time_available = 0;
break;
}
time_available -= time_taken;
}
// update number of spare microseconds
_spare_micros += time_available;
_spare_ticks++;
if (_spare_ticks == 32)
{
_spare_ticks /= 2;
_spare_micros /= 2;
}
}
最多運行了49個任務
注意：run()函式有看不明白的自己可以採用串列埠列印驗證。