版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lixiaoweimashixiao/article/details/83020681
目录
文章目录
摘要
本节主要记录自己学习Ardupilot的 chibios代码过程，这里是从应用层main()函数实现开始讲解。后续将会讲解飞控上电执行的第一条指令，并且怎么运转到main()函数的过程，欢迎批评指正。
0序言
apm飞控的固件ardupilot，采用Nuttx操作系统，最新的代码已经采用Chibios，本人已经验证，效果不错！
习惯了MDK-keil的朋友，喜欢上来直接找main()函数，记得我刚开始学STM32时，也是如此，后续有很多疑惑(单片机一上电怎么运行的？第一条代码从哪里执行？我点击keil上的编译按钮就可以直接编译自己写的代码，upload就可以直接下载到单片机,为什么，这是什么鬼？？？)，现在想想上大学时，我最讨厌的就是汇编老师教的汇编，不喜欢他的教课方式，误人子弟！！！因此大学嵌入式这鬼一点没学好，一句话基础没打牢固也怪不得别人！后续随着自己知识增多，认识的增长，意识到汇编是不可跳过的卡，其重要性，只能三个!!! 自己没事的时候就学习汇编，看了王爽老师的汇编收获巨大，对汇编不在恐惧。但是ardupilot不能按这种方式。MDK-KEIL虽然给我们的编程带来了方便，但是也让我们失去接触很多核心的东西，开发商帮我们写好了启动代码，为我们的编程省了很多事情。事情总是利弊相联系，带来好处 同时，也带来了不利的一面。 会让我们变成傻瓜因此单片机一上电如何运转，代码如何进行编译，都是我们不可不学的重点，要想走得更远，汇编语言，ARM相关的知识，启动文件，Linux系统，C++语言，makefile,shell是不可不学的嵌入式宝贵知识。
记得自己刚开始学习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()函数有看不明白的自己可以采用串口打印验证。