ArduPilot开源飞控之Copter任务

1. 源由

之前研读ArduPilot开源飞控代码更多是从静态角度：工程结构、代码框架、front/backend分层结构等。

上述方法也是最为基本的一种逻辑理解方式，但也缺少一种动态方面的整体设计。

为此，本章节将从另一个动态任务的角度切入，整体上看下系统是如何运行的。

2. 整体分析

分析的总体切入点，从ArduPilot之开源代码Task介绍开始。

ArduCopter/Copter.cppconst AP_Scheduler::Task Copter::scheduler_tasks[]数组包含了所用运行时的任务。

/*
  scheduler table - all tasks should be listed here.

  All entries in this table must be ordered by priority.

  This table is interleaved with the table in AP_Vehicle to determine
  the order in which tasks are run.  Convenience methods SCHED_TASK
  and SCHED_TASK_CLASS are provided to build entries in this structure:

SCHED_TASK arguments:
 - name of static function to call
 - rate (in Hertz) at which the function should be called
 - expected time (in MicroSeconds) that the function should take to run
 - priority (0 through 255, lower number meaning higher priority)

SCHED_TASK_CLASS arguments:
 - class name of method to be called
 - instance on which to call the method
 - method to call on that instance
 - rate (in Hertz) at which the method should be called
 - expected time (in MicroSeconds) that the method should take to run
 - priority (0 through 255, lower number meaning higher priority)

 */
const AP_Scheduler::Task Copter::scheduler_tasks[] = {
    // update INS immediately to get current gyro data populated
    FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
    // run low level rate controllers that only require IMU data
    FAST_TASK(run_rate_controller),
#if AC_CUSTOMCONTROL_MULTI_ENABLED == ENABLED
    FAST_TASK(run_custom_controller),
#endif
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME
    // send outputs to the motors library immediately
    FAST_TASK(motors_output),
     // run EKF state estimator (expensive)
    FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAME
    FAST_TASK(update_heli_control_dynamics),
#endif //HELI_FRAME
    // Inertial Nav
    FAST_TASK(read_inertia),
    // check if ekf has reset target heading or position
    FAST_TASK(check_ekf_reset),
    // run the attitude controllers
    FAST_TASK(update_flight_mode),
    // update home from EKF if necessary
    FAST_TASK(update_home_from_EKF),
    // check if we've landed or crashed
    FAST_TASK(update_land_and_crash_detectors),
    // surface tracking update
    FAST_TASK(update_rangefinder_terrain_offset),
#if HAL_MOUNT_ENABLED
    // camera mount's fast update
    FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast),
#endif
    FAST_TASK(Log_Video_Stabilisation),

    SCHED_TASK(rc_loop,              250,    130,  3),
    SCHED_TASK(throttle_loop,         50,     75,  6),
    SCHED_TASK_CLASS(AP_GPS,               &copter.gps,                 update,          50, 200,   9),
#if AP_OPTICALFLOW_ENABLED
    SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),
#endif
    SCHED_TASK(update_batt_compass,   10,    120, 15),
    SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all,    10,  50,  18),
    SCHED_TASK(arm_motors_check,      10,     50, 21),
#if TOY_MODE_ENABLED == ENABLED
    SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),
#endif
    SCHED_TASK(auto_disarm_check,     10,     50,  27),
    SCHED_TASK(auto_trim,             10,     75,  30),
#if RANGEFINDER_ENABLED == ENABLED
    SCHED_TASK(read_rangefinder,      20,    100,  33),
#endif
#if HAL_PROXIMITY_ENABLED
    SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),
#endif
#if BEACON_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),
#endif
    SCHED_TASK(update_altitude,       10,    100,  42),
    SCHED_TASK(run_nav_updates,       50,    100,  45),
    SCHED_TASK(update_throttle_hover,100,     90,  48),
#if MODE_SMARTRTL_ENABLED == ENABLED
    SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),
#endif
#if HAL_SPRAYER_ENABLED
    SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),
#endif
    SCHED_TASK(three_hz_loop,          3,     75, 57),
    SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),
    SCHED_TASK_CLASS(AP_Baro,              &copter.barometer,             accumulate,    50,  90,  63),
#if PRECISION_LANDING == ENABLED
    SCHED_TASK(update_precland,      400,     50,  69),
#endif
#if FRAME_CONFIG == HELI_FRAME
    SCHED_TASK(check_dynamic_flight,  50,     75,  72),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
#endif
    SCHED_TASK_CLASS(AP_Notify,            &copter.notify,              update,          50,  90,  78),
    SCHED_TASK(one_hz_loop,            1,    100,  81),
    SCHED_TASK(ekf_check,             10,     75,  84),
    SCHED_TASK(check_vibration,       10,     50,  87),
    SCHED_TASK(gpsglitch_check,       10,     50,  90),
    SCHED_TASK(takeoff_check,         50,     50,  91),
#if AP_LANDINGGEAR_ENABLED
    SCHED_TASK(landinggear_update,    10,     75,  93),
#endif
    SCHED_TASK(standby_update,        100,    75,  96),
    SCHED_TASK(lost_vehicle_check,    10,     50,  99),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),
    SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_send,    400, 550, 105),
#if HAL_MOUNT_ENABLED
    SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),
#endif
#if AP_CAMERA_ENABLED
    SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),
#endif
#if LOGGING_ENABLED == ENABLED
    SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
    SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
    SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),
#endif
    SCHED_TASK_CLASS(AP_InertialSensor,    &copter.ins,                 periodic,       400,  50, 123),

    SCHED_TASK_CLASS(AP_Scheduler,         &copter.scheduler,           update_logging, 0.1,  75, 126),
#if AP_RPM_ENABLED
    SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),
#endif
    SCHED_TASK_CLASS(AP_TempCalibration,   &copter.g2.temp_calibration, update,          10, 100, 135),
#if HAL_ADSB_ENABLED
    SCHED_TASK(avoidance_adsb_update, 10,    100, 138),
#endif
#if ADVANCED_FAILSAFE == ENABLED
    SCHED_TASK(afs_fs_check,          10,    100, 141),
#endif
#if AP_TERRAIN_AVAILABLE
    SCHED_TASK(terrain_update,        10,    100, 144),
#endif
#if AP_GRIPPER_ENABLED
    SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75, 147),
#endif
#if AP_WINCH_ENABLED
    SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),
#endif
#ifdef USERHOOK_FASTLOOP
    SCHED_TASK(userhook_FastLoop,    100,     75, 153),
#endif
#ifdef USERHOOK_50HZLOOP
    SCHED_TASK(userhook_50Hz,         50,     75, 156),
#endif
#ifdef USERHOOK_MEDIUMLOOP
    SCHED_TASK(userhook_MediumLoop,   10,     75, 159),
#endif
#ifdef USERHOOK_SLOWLOOP
    SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),
#endif
#ifdef USERHOOK_SUPERSLOWLOOP
    SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),
#endif
#if HAL_BUTTON_ENABLED
    SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),
#endif
#if STATS_ENABLED == ENABLED
    SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100, 171),
#endif
};

3. 必须任务

3.1 FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update)

详见：ArduPilot开源代码之AP_InertialSensor

3.2 FAST_TASK(run_rate_controller)

待补充

3.3 FAST_TASK(motors_output)

待补充

3.4 FAST_TASK(read_AHRS)

详见：ArduPilot开源飞控之AP_AHRS

void Copter::read_AHRS(void)
{
    // we tell AHRS to skip INS update as we have already done it in FAST_TASK.
    ahrs.update(true);
}

3.5 FAST_TASK(read_inertia)

详见：ArduPilot开源飞控之AP_InertialNav

3.6 FAST_TASK(check_ekf_reset)

待补充

3.7 FAST_TASK(update_flight_mode)

待补充

3.8 FAST_TASK(update_home_from_EKF)

待补充

3.9 FAST_TASK(update_land_and_crash_detectors)

待补充

3.10 FAST_TASK(update_rangefinder_terrain_offset)

待补充

3.11 FAST_TASK(Log_Video_Stabilisation)

待补充

3.12 SCHED_TASK(rc_loop, 250, 130, 3)

待补充

3.13 SCHED_TASK(throttle_loop, 50, 75, 6)

待补充

3.14 SCHED_TASK_CLASS(AP_GPS, &copter.gps, update, 50, 200, 9)

详见：ArduPilot开源代码之AP_GPS

3.15 SCHED_TASK(update_batt_compass, 10, 120, 15)

待补充

3.16 SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&copter.g2.rc_channels, read_aux_all, 10, 50, 18)

待补充

3.17 SCHED_TASK(arm_motors_check, 10, 50, 21)

待补充

3.18 SCHED_TASK(auto_disarm_check, 10, 50, 27)

待补充

3.19 SCHED_TASK(auto_trim, 10, 75, 30)

待补充

3.20 SCHED_TASK(update_altitude, 10, 100, 42)

待补充

3.21 SCHED_TASK(run_nav_updates, 50, 100, 45)

待补充

3.22 SCHED_TASK(update_throttle_hover,100, 90, 48)

待补充

3.23 SCHED_TASK(three_hz_loop, 3, 75, 57)

待补充

3.24 SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90, 63)

详见：ArduPilot开源飞控之AP_Baro

3.25 SCHED_TASK(one_hz_loop, 1, 100, 81)

待补充

3.26 SCHED_TASK(ekf_check, 10, 75, 84)

待补充

3.27 SCHED_TASK(check_vibration, 10, 50, 87),

待补充

3.28 SCHED_TASK(gpsglitch_check, 10, 50, 90),

待补充

3.29 SCHED_TASK(takeoff_check, 50, 50, 91),

待补充

3.30 SCHED_TASK(standby_update, 100, 75, 96),

待补充

3.31 SCHED_TASK(lost_vehicle_check, 10, 50, 99),

待补充

3.32 SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_receive, 400, 180, 102),

待补充

3.33 SCHED_TASK_CLASS(GCS, (GCS*)&copter._gcs, update_send, 400, 550, 105),

待补充

3.34 SCHED_TASK_CLASS(AP_InertialSensor, &copter.ins, periodic, 400, 50, 123),

详见：ArduPilot开源代码之AP_InertialSensor

3.35 SCHED_TASK_CLASS(AP_Scheduler, &copter.scheduler, update_logging, 0.1, 75, 126),

待补充

3.36 SCHED_TASK_CLASS(AP_TempCalibration, &copter.g2.temp_calibration, update, 10, 100, 135),

待补充

4. 可选任务

4.1 AC_CUSTOMCONTROL_MULTI_ENABLED

FAST_TASK(run_custom_controller)

待补充

4.2 HAL_MOUNT_ENABLED

// camera mount's fast update
FAST_TASK_CLASS(AP_Mount, &copter.camera_mount, update_fast)

待补充

4.3 AP_OPTICALFLOW_ENABLED

SCHED_TASK_CLASS(AP_OpticalFlow,          &copter.optflow,             update,         200, 160,  12),

待补充

4.4 TOY_MODE_ENABLED

SCHED_TASK_CLASS(ToyMode,              &copter.g2.toy_mode,         update,          10,  50,  24),

待补充

4.5 RANGEFINDER_ENABLED

SCHED_TASK(read_rangefinder,      20,    100,  33),

待补充

4.6 HAL_PROXIMITY_ENABLED

SCHED_TASK_CLASS(AP_Proximity,         &copter.g2.proximity,        update,         200,  50,  36),

待补充

4.7 AP_BEACON_ENABLED

SCHED_TASK_CLASS(AP_Beacon,            &copter.g2.beacon,           update,         400,  50,  39),

待补充

4.8 MODE_SMARTRTL_ENABLED

SCHED_TASK_CLASS(ModeSmartRTL,         &copter.mode_smartrtl,       save_position,    3, 100,  51),

待补充

4.9 HAL_SPRAYER_ENABLED

SCHED_TASK_CLASS(AC_Sprayer,           &copter.sprayer,               update,         3,  90,  54),

待补充

4.10 AP_SERVORELAYEVENTS_ENABLED

SCHED_TASK_CLASS(AP_ServoRelayEvents,  &copter.ServoRelayEvents,      update_events, 50,  75,  60),

待补充

4.11 AC_PRECLAND_ENABLED

SCHED_TASK(update_precland,      400,     50,  69),

待补充

4.12 LOGGING_ENABLED

SCHED_TASK(loop_rate_logging, LOOP_RATE,    50,  75),
SCHED_TASK(ten_hz_logging_loop,   10,    350, 114),
SCHED_TASK(twentyfive_hz_logging, 25,    110, 117),
SCHED_TASK_CLASS(AP_Logger,            &copter.logger,              periodic_tasks, 400, 300, 120),

待补充

4.13 AP_LANDINGGEAR_ENABLED

SCHED_TASK(landinggear_update,    10,     75,  93),

待补充

4.14 HAL_MOUNT_ENABLED

SCHED_TASK_CLASS(AP_Mount,             &copter.camera_mount,        update,          50,  75, 108),

待补充

4.15 AP_CAMERA_ENABLED

SCHED_TASK_CLASS(AP_Camera,            &copter.camera,              update,          50,  75, 111),

待补充

4.16 AP_RPM_ENABLED

SCHED_TASK_CLASS(AP_RPM,               &copter.rpm_sensor,          update,          40, 200, 129),

待补充

4.17 HAL_ADSB_ENABLED

SCHED_TASK(avoidance_adsb_update, 10,    100, 138),

待补充

4.18 ADVANCED_FAILSAFE

SCHED_TASK(afs_fs_check,          10,    100, 141),

待补充

4.19 AP_TERRAIN_AVAILABLE

SCHED_TASK(terrain_update,        10,    100, 144),

待补充

4.20 AP_GRIPPER_ENABLED

SCHED_TASK_CLASS(AP_Gripper,           &copter.g2.gripper,          update,          10,  75, 147),

待补充

4.21 AP_WINCH_ENABLED

SCHED_TASK_CLASS(AP_Winch,             &copter.g2.winch,            update,          50,  50, 150),

待补充

4.22 USERHOOK_FASTLOOP

SCHED_TASK(userhook_FastLoop,    100,     75, 153),

待补充

4.23 USERHOOK_50HZLOOP

SCHED_TASK(userhook_50Hz,         50,     75, 156),

待补充

4.24 USERHOOK_MEDIUMLOOP

SCHED_TASK(userhook_MediumLoop,   10,     75, 159),

待补充

4.25 USERHOOK_SLOWLOOP

SCHED_TASK(userhook_SlowLoop,      3.3,   75, 162),

待补充

4.26 USERHOOK_SUPERSLOWLOOP

SCHED_TASK(userhook_SuperSlowLoop, 1,     75, 165),

待补充

4.27 HAL_BUTTON_ENABLED

SCHED_TASK_CLASS(AP_Button,            &copter.button,              update,           5, 100, 168),

待补充

4.28 STATS_ENABLED

SCHED_TASK_CLASS(AP_Stats,             &copter.g2.stats,            update,           1, 100, 171),

待补充

5. HELI Copter任务

FRAME_CONFIG == HELI_FRAME

FAST_TASK(update_heli_control_dynamics)
FAST_TASK(heli_update_autorotation)
SCHED_TASK(check_dynamic_flight,  50,     75,  72),

注：暂不展开。

6. 总结

这里主要从业务逻辑角度整理的任务，其中还有驱动任务。

对比ArduPilot开源飞控系统之简单介绍，就能看出为什么很多业务逻辑面尚无法很好的关联，串联起来理解的原因。

后续将会逐步补充全前面关于【待补充】的相关研读内容。

7. 参考资料

【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码Task介绍
【3】ArduPilot飞控启动&运行过程简介
【4】ArduPilot之开源代码Library&Sketches设计
【5】ArduPilot之开源代码Sensor Drivers设计