logo头像
Snippet 博客主题

Ardupilot chibios study notes

2019/02/16

主回调函数入口:‘AP_HAL_MAIN_CALLBACKS(&copter);’ 

1
2
3
4
5
6
7
#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; \
	} \
	}

run函数创建了main_loop线程函数，该线程一定会创建很多任务？？！带着这个问题看代码：初始化—创建任务—

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
 void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
{
//ChibiOS HAL初始化，这也初始化配置的设备驱动程序。并执行特定的板层初始化：内核初始化，main()函数变成一个线程，RTOS是运行的。

#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,     /* 线程的优先级 */
                      main_loop,             /* 函数体，这里是我们最关心的函数 */
                      nullptr);              /* Thread parameter.    */
    chThdExit(0);
}

const AP_HAL::HAL& AP_HAL::get_HAL() {
    static const HAL_ChibiOS hal_chibios;
    return hal_chibios;
}

注意’AP_HAL_MAIN();’ 此函数已经不再使用。

主回调函数入口：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
static THD_FUNCTION(main_loop,arg)
{
    daemon_task = chThdGetSelfX();

#ifdef HAL_I2C_CLEAR_BUS  // 此部分为一些I2C设备会导致程序卡住时候，就需要用此I2C总线清除。
    // 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初始化
    
    hal.uartA->begin(115200);    // uart0的波特率设置为115200

#ifdef HAL_SPI_CHECK_CLOCK_FREQ
    // optional test of SPI clock frequencies 测试SPI时钟频率
    ChibiOS::SPIDevice::test_clock_freq();
#endif 

    //Setup SD Card and Initialise FATFS bindings
    sdcard_init(); // 初始化SD卡和文件系统

    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
     */
    hal_chibios_set_priority(APM_STARTUP_PRIORITY); // chibios设置APM启动优先级

    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()) {
            hal.scheduler->delay_microseconds(250);
        }
    }
    thread_running = false;
}

此部分为将需要用到的进程分优先级来初始化。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
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.    */
                     _timer_thread,             /* Thread function.     */
                     this);                     /* Thread parameter.    */

    // 初始化UAVCAN进程----setup the uavcan thread - this will call tasks at 1kHz
#if HAL_WITH_UAVCAN
    _uavcan_thread_ctx = chThdCreateStatic(_uavcan_thread_wa,
                     sizeof(_uavcan_thread_wa),
                     APM_UAVCAN_PRIORITY,        /* Initial priority.    */
                     _uavcan_thread,            /* Thread function.     */
                     this);                     /* Thread parameter.    */
#endif
    // 设置遥控器输入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.    */
                     _rcin_thread,             /* Thread function.     */
                     this);                     /* Thread parameter.    */

    // the toneAlarm thread runs at a medium priority
#ifdef HAL_PWM_ALARM
    _toneAlarm_thread_ctx = chThdCreateStatic(_toneAlarm_thread_wa,
                     sizeof(_toneAlarm_thread_wa),
                     APM_TONEALARM_PRIORITY,        /* Initial priority.    */
                     _toneAlarm_thread,             /* Thread function.     */
                     this);                    /* Thread parameter.    */
#endif
    // 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.      */

    // 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.      */
}

基本上应用层的初始化都在这里

g_callbacks->setup();

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
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的循环，数组表共有58个需要执行的任务，通过设置enable或者disable启动或者禁止任务。此scheduler_tasks[0]加载很多任务进来。
    scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}
// 如下：

/*
  scheduler table for fast CPUs - all regular tasks apart from the fast_loop()
  should be listed here, along with how often they should be called (in hz)
  and the maximum time they are expected to take (in microseconds)
 */
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),
#if OPTFLOW == ENABLED
    SCHED_TASK(update_optical_flow,  200,    160),
#endif
    SCHED_TASK(update_batt_compass,   10,    120),
    SCHED_TASK(read_aux_switches,     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(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
    SCHED_TASK(three_hz_loop,          3,     75),
    SCHED_TASK(compass_accumulate,   100,    100),
    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),
    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_check_input,      400,    180),
    SCHED_TASK(gcs_send_heartbeat,     1,    110),
	SCHED_TASK(mmc_can_protocol,      50,    50),
    SCHED_TASK(gcs_send_deferred,     50,    550),
    SCHED_TASK(gcs_data_stream_send,  50,    550),
#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
};

初始化这么多任务，那么怎么被使用呢？
通过 g_callbacks->loop()在主loop里面被调用。

1
2
3
4
5
 void Copter::loop()
{
    scheduler.loop();
    G_Dt = scheduler.get_last_loop_time_s();
}
  • Ardupilot chibios编译，启动，main函数学习
  • Ardupilot chibios IO固件,IO与FMU通信,固件下载
Copyright © 2017 | Powered by Hexo | Theme by Snippet