APM_Rover运行纲领分析，以pixhawk-fmuv2为硬件平台，ChibiOS为底层操作系统:

1. 纯抽象载具类: class AP_Vehicle

class AP_Vehicle 派生自AP_HAL::HAL::Callbacks接口类，通常从接口类派生是希望子类具有某种期望的行为，这里AP_Vehicle拥有setup和loop的行为；此类是所有具象载具(Plane,Copter,Sub,Rover,Tracker)的基类；

class AP_Vehicle : public AP_HAL::HAL::Callbacks {
public:AP_Vehicle() { --->>> 构造方法，确保单例if (_singleton) {AP_HAL::panic("Too many Vehicles");}_singleton = this;}/* Do not allow copies */ --->>> 指示编译器弃用拷贝构造和拷贝赋值AP_Vehicle(const AP_Vehicle &other) = delete;AP_Vehicle &operator=(const AP_Vehicle&) = delete;static AP_Vehicle *get_singleton(); --->>> 静态成员函数，获取单例--->>> 纯虚接口 set_mode, 期望子类具有set_mode的行为 <<<---bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0;/*common parameters for fixed wing aircraft --->>> 固定翼飞行器的公参*/struct FixedWing {AP_Int8 throttle_min;-->>> 油门上下限，转换率，巡航AP_Int8 throttle_max; AP_Int8 throttle_slewrate;AP_Int8 throttle_cruise;AP_Int8 takeoff_throttle_max; --->>> 起飞油门最大值AP_Int16 airspeed_min; --->>> 空速上下限，巡航AP_Int16 airspeed_max;AP_Int32 airspeed_cruise_cm;AP_Int32 min_gndspeed_cm; --->>> 最小地速AP_Int8  crash_detection_enable; --->>> 坠机检测使能AP_Int16 roll_limit_cd; --->>> 横滚上下限AP_Int16 pitch_limit_max_cd;AP_Int16 pitch_limit_min_cd;        AP_Int8  autotune_level; --->>> 自动调制级别AP_Int8  stall_prevention; --->>> 防失速AP_Int16 loiter_radius; --->>> 刷锅半径struct Rangefinder_State { --->>> 测距仪bool in_range:1; --->>> 在量程内bool have_initial_reading:1; --->>> 有初值bool in_use:1; --->>> 使用中float initial_range; --->>> 初始范围float correction; --->>> 校正float initial_correction; --->>> 初始校正float last_stable_correction; --->>> 最近一次稳定校正uint32_t last_correction_time_ms; --->>> 最后一次校正时间uint8_t in_range_count; --->>> 在量程内计数float height_estimate; --->>> 高度估计float last_distance; --->>> 最近一次的距离信息};// stages of flight --->>> 飞行阶段enum FlightStage {FLIGHT_TAKEOFF       = 1, --->>> 起飞FLIGHT_VTOL          = 2, --->>> 垂直起降FLIGHT_NORMAL        = 3, --->>> 正常FLIGHT_LAND          = 4, --->>> 着陆FLIGHT_ABORT_LAND    = 7  --->>> 终止着陆};};/*common parameters for multicopters --->>> 多旋翼公参*/struct MultiCopter {AP_Int16 angle_max; --->>> 最大角度};protected:// board specific config ---> 板子相关AP_BoardConfig BoardConfig;#if HAL_WITH_UAVCAN// board specific config for CAN busAP_BoardConfig_CAN BoardConfig_CAN;
#endif// sensor drivers --->>> 传感器驱动, gps, 气压，磁力，惯性，按键，距离，信号强度，串口，中继，通知AP_GPS gps;AP_Baro barometer;Compass compass;AP_InertialSensor ins;AP_Button button;RangeFinder rangefinder;AP_RSSI rssi;AP_SerialManager serial_manager;AP_Relay relay;AP_ServoRelayEvents ServoRelayEvents;// notification object for LEDs, buzzers etc (parameter set to// false disables external leds) --->>> 通知设备，led,buzzer等AP_Notify notify;private:static AP_Vehicle *_singleton; --->>> 静态成员变量，单例模式};namespace AP {AP_Vehicle *vehicle();
};

2. Rover类型:

Rover派生自上面的抽象载具类，实现了set_up和loop接口方法，实现了set_mode

class Rover : public AP_Vehicle {
public:friend class GCS_MAVLINK_Rover;friend class Parameters;friend class ParametersG2;friend class AP_Rally_Rover;friend class AP_Arming_Rover;
#if ADVANCED_FAILSAFE == ENABLEDfriend class AP_AdvancedFailsafe_Rover;
#endiffriend class GCS_Rover;friend class Mode;friend class ModeAcro;friend class ModeAuto;friend class ModeGuided;friend class ModeHold;friend class ModeLoiter;friend class ModeSteering;friend class ModeManual;friend class ModeRTL;friend class ModeSmartRTL;friend class ModeFollow;friend class ModeSimple;friend class RC_Channel_Rover;friend class RC_Channels_Rover;friend class Sailboat;Rover(void);// HAL::Callbacks implementation. --->>> 实现setuo和loop方法，落实行为void setup(void) override;void loop(void) override;private:// must be the first AP_Param variable declared to ensure its// constructor runs before the constructors of the other AP_Param// variables --->>> 确保在其他参数之前构造AP_Param param_loader;// all settable parametersParameters g;ParametersG2 g2;// main loop scheduler--->>> 主循环调度器AP_Scheduler scheduler;// mapping between input channels --->>> 输入通道映射RCMapper rcmap;// primary control channels --->>> 主要的控制通道RC_Channel *channel_steer;RC_Channel *channel_throttle;RC_Channel *channel_lateral;AP_Logger logger;// flight modes convenience arrayAP_Int8 *modes;const uint8_t num_modes = 6;// AP_RPM Module --->>> 转速模块AP_RPM rpm_sensor;// Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLENavEKF2 EKF2{&ahrs, rangefinder};NavEKF3 EKF3{&ahrs, rangefinder};AP_AHRS_NavEKF ahrs{EKF2, EKF3};
#elseAP_AHRS_DCM ahrs;
#endif// Arming/Disarming management classAP_Arming_Rover arming;AP_L1_Control L1_controller{ahrs, nullptr}; --->>> L1 辅助驾驶#if AP_AHRS_NAVEKF_AVAILABLEOpticalFlow optflow;
#endif#if OSD_ENABLED == ENABLEDAP_OSD osd;
#endif#if CONFIG_HAL_BOARD == HAL_BOARD_SITL --->>> 闭环仿真SITL::SITL sitl;
#endif// GCS handling --->>> 地面控制站GCS_Rover _gcs;  // avoid using this; use gcs()GCS_Rover &gcs() { return _gcs; }// RC Channels: --->>> 遥控通道RC_Channels_Rover &rc() { return g2.rc_channels; }// The rover's current locationstruct Location current_loc;// Camera
#if CAMERA == ENABLEDAP_Camera camera{MASK_LOG_CAMERA, current_loc};
#endif// Camera/Antenna mount tracking and stabilisation stuff
#if MOUNT == ENABLEDAP_Mount camera_mount;
#endif// true if initialisation has completed--->>> 初始化完成标志bool initialised;// This is the state of the flight control system --->>> 飞行控制系统的状态// There are multiple states defined such as MANUAL, AUTO, ...Mode *control_mode;ModeReason control_mode_reason = ModeReason::UNKNOWN;// Used to maintain the state of the previous control switch position// This is set to -1 when we need to re-read the switchuint8_t oldSwitchPosition;// structure for holding failsafe state 失速状态struct {uint8_t bits;               // bit flags of failsafes that have started (but not necessarily triggered an action)uint32_t start_time;        // start time of the earliest failsafeuint8_t triggered;          // bit flags of failsafes that have triggered an actionuint32_t last_valid_rc_ms;  // system time of most recent RC input from pilot --->>> 最近遥控输入时间uint32_t last_heartbeat_ms; // system time of most recent heartbeat from ground station --->>> 最近来自地面站的心跳时间bool ekf;} failsafe;// true if we have a position estimate from AHRS --->>> 是否有一个AHRS的姿态估计bool have_position;// range finder last update (used for DPTH logging)uint32_t rangefinder_last_reading_ms;// Ground speed// The amount current ground speed is below min ground speed.  meters per second --->>> 地速 m/sfloat ground_speed;// Battery Sensors --->>> 这里使用了&Rover::handle_battery_failsafe成员函数指针AP_BattMonitor battery{MASK_LOG_CURRENT,FUNCTOR_BIND_MEMBER(&Rover::handle_battery_failsafe, void, const char*, const int8_t),_failsafe_priorities};// true if the compass's initial location has been set --->>> 指南针初始位置标志bool compass_init_location;// IMU variables// The main loop execution time.  Seconds// This is the time between calls to the DCM algorithm and is the Integration time for the gyros.--->>> main loop 执行时间，即: 两次调用DCM算法和陀螺仪融合的间隔时间float G_Dt;// flyforward timer 前向飞行定时器uint32_t flyforward_start_ms;static const AP_Scheduler::Task scheduler_tasks[]; --->>> 静态成员 调度器任务数组static const AP_Param::Info var_info[]; --->>> 静态成员 参数信息数组static const LogStructure log_structure[]; --->>> 静态成员 日志结构// time that rudder/steering arming has been runninguint32_t rudder_arm_timer; --->>> 方向舵机转向器设备运行时间// Store the time the last GPS message was received.uint32_t last_gps_msg_ms{0}; --->>> 上一次GPS接收到的时间// latest wheel encoder values --->>> 最近一次的轮子编码器值--->>> 报告给地面站的总计编码器记录的距离float wheel_encoder_last_distance_m[WHEELENCODER_MAX_INSTANCES];    // total distance recorded by wheel encoder (for reporting to GCS)bool wheel_encoder_initialised;                                     // true once arrays below have been initialised to sensors initial values--->>> 上一次更新到EKF的弧度距离float wheel_encoder_last_angle_rad[WHEELENCODER_MAX_INSTANCES];     // distance in radians at time of last update to EKF--->>> 上次读取每个编码器的系统时间uint32_t wheel_encoder_last_reading_ms[WHEELENCODER_MAX_INSTANCES]; // system time of last ping from each encoder--->>> 标识最近发送给EKF的编码器uint8_t wheel_encoder_last_index_sent;                              // index of the last wheel encoder sent to the EKF// True when we are doing motor test --->>> 标识我们真在进行motor检测bool motor_test;--->>> 模式ModeInitializing mode_initializing; --->>> 初始化模式ModeHold mode_hold; --->>> 保持模式ModeManual mode_manual; --->>> 手动模式ModeAcro mode_acro;   --->>> 特技模式ModeGuided mode_guided; --->>> 导航模式ModeAuto mode_auto; --->>> 自动模式ModeLoiter mode_loiter; --->>> 悬停模式ModeSteering mode_steering; --->>> 转向模式ModeRTL mode_rtl; --->>> 自返模式ModeSmartRTL mode_smartrtl; -->>> 智能自返ModeFollow mode_follow; --->>> 跟随模式ModeSimple mode_simple; --->>> 简单模式// cruise throttle and speed learning --->>> 巡航油门和速度typedef struct {LowPassFilterFloat speed_filt = LowPassFilterFloat(2.0f);LowPassFilterFloat throttle_filt = LowPassFilterFloat(2.0f);uint32_t learn_start_ms;uint32_t log_count;} cruise_learn_t;cruise_learn_t cruise_learn;private:// APMrover2.cppvoid stats_update(); --->>> 统计更新void ahrs_update(); --->>> 航姿参考系统更新void gcs_failsafe_check(void); --->>> 地面控制站失速检测void update_logging1(void); --->>> 更新日志void update_logging2(void);void one_second_loop(void); --->>> 1s 循环void update_GPS(void); --->>> GPS更新void update_current_mode(void); --->>> 当前模式更新void update_mission(void); --->>> 任务更新// balance_bot.cpp --->>> 根据油门设置期望的俯仰角void balancebot_pitch_control(float &throttle);bool is_balancebot() const;// commands.cpp --->>> 设置初始位置bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;void update_home();// compat.cppvoid delay(uint32_t ms);// crash_check.cpp --->>> 坠毁检测void crash_check();// cruise_learn.cpp  --->>> 巡航void cruise_learn_start();void cruise_learn_update();void cruise_learn_complete();void log_write_cruise_learn();// ekf_check.cpp --->>> EKF检测void ekf_check();bool ekf_over_threshold();bool ekf_position_ok();void failsafe_ekf_event();void failsafe_ekf_off_event(void);// failsafe.cpp --->>> 失速保护void failsafe_trigger(uint8_t failsafe_type, bool on);void handle_battery_failsafe(const char* type_str, const int8_t action);
#if ADVANCED_FAILSAFE == ENABLEDvoid afs_fs_check(void);
#endif// fence.cpp --->>> 围栏检测void fence_check();// GCS_Mavlink.cpp --->>> 地面控制站mavlink，发送伺服输出和轮子编码器距离void send_servo_out(mavlink_channel_t chan);void send_wheel_encoder_distance(mavlink_channel_t chan);// Log.cpp --->>> 日志相关void Log_Write_Attitude(); --->>> 高度void Log_Write_Depth(); --->>> 深度void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); --->>> 导引目标void Log_Write_Nav_Tuning(); --->>> 导航调制void Log_Write_Sail(); --->>> 航行void Log_Write_Startup(uint8_t type); --->>> 启动void Log_Write_Steering(); --->>> 转向void Log_Write_Throttle(); --->>> 油门void Log_Write_RC(void); --->>> 遥控void Log_Write_Vehicle_Startup_Messages(); --->>> 载具启动消息void Log_Read(uint16_t log_num, uint16_t start_page, uint16_t end_page);--->>>读void log_init(void); --->>> 初始化// mode.cpp --->>>模式Mode *mode_from_mode_num(enum Mode::Number num);// Parameters.cpp --->>> 参数加载void load_parameters(void);// radio.cpp --->>> 无线电void set_control_channels(void); --->>> 设置控制通道void init_rc_in(); --->>> 初始化遥控器输入void rudder_arm_disarm_check(); --->>> 舵机战备和解除检查void read_radio(); --->>> 读无线电void radio_failsafe_check(uint16_t pwm); --->>> 无线电失速检查bool trim_radio(); // sensors.cpp --->>> 传感器void update_compass(void); --->>> 更新磁力计void compass_save(void); --->>> 磁力计保存void init_beacon(); --->>> 初始化 指示灯void init_visual_odom(); --->>> 初始化虚拟里程计void update_wheel_encoder(); --->>> 更新轮子编码器void accel_cal_update(void); --->>> 加速度计计算更新void read_rangefinders(void); --->>> 读取测距仪void init_proximity(); --->>> 初始化接近传感器void read_airspeed(); --->>> 空速计void rpm_update(void); --->>> 转速更新// Steering.cpp --->>> 舵机转向void set_servos(void); 设置舵机// system.cpp --->>> 系统相关void init_ardupilot(); --->>> ardupilot初始化void startup_ground(void); --->>> 地面启动void update_ahrs_flyforward(); --->>> 更新航姿参考系统前向飞行bool set_mode(Mode &new_mode, ModeReason reason); --->>> 设置模式bool set_mode(const uint8_t new_mode, ModeReason reason) override; --->>> 设置模式bool mavlink_set_mode(uint8_t mode); --->>> mavlink设置模式void startup_INS_ground(void); --->>> 地面启动惯导void notify_mode(const Mode *new_mode); --->>> 通知模式uint8_t check_digital_pin(uint8_t pin); --->>> 检查数字引脚bool should_log(uint32_t mask); --->>> 应当载入日志bool is_boat() const; --->>> 是船吗？#if OSD_ENABLED == ENABLEDvoid publish_osd_info(); --->>> 发布屏显信息
#endifenum Failsafe_Action { --->>> 失速保护行为Failsafe_Action_None          = 0, --->>> 啥都不做Failsafe_Action_RTL           = 1, --->>> 返回出发点Failsafe_Action_Hold          = 2, --->>> 保持Failsafe_Action_SmartRTL      = 3, --->>> 智能返回Failsafe_Action_SmartRTL_Hold = 4, --->>> 智能返回并保持Failsafe_Action_Terminate     = 5 --->>> 终止};enum class Failsafe_Options : uint32_t {Failsafe_Option_Active_In_Hold = (1<<0)};--->>> 静态常量表达式，失速优先级顺序static constexpr int8_t _failsafe_priorities[] = { Failsafe_Action_Terminate,Failsafe_Action_Hold,Failsafe_Action_RTL,Failsafe_Action_SmartRTL_Hold,Failsafe_Action_SmartRTL,Failsafe_Action_None,-1 // the priority list must end with a sentinel of -1};static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,"_failsafe_priorities is missing the sentinel");public:void mavlink_delay_cb(); --->>> mavlink延时回调void failsafe_check(); --->>> 失速检查// Motor testvoid motor_test_output(); --->>> 马达测试输出bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value); --->>> mavlink马达测试输出MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, AP_MotorsUGV::motor_test_order motor_instance, uint8_t throttle_type, int16_t throttle_value, float timeout_sec);void motor_test_stop(); --->>> 马达测试停止// frame type --->>> 机架类型uint8_t get_frame_type() { return g2.frame_type.get(); }AP_WheelRateControl& get_wheel_rate_control() { return g2.wheel_rate_control; } --->>> 轮速控制// Simple modefloat simple_sin_yaw;
};

3. 从setup入手：

void setup(void) override;

/*setup is called when the sketch starts*/
void Rover::setup()
{// load the default values of variables listed in var_info[]--->>> 调用静态方法，使用EEPROM中的默认设置AP_Param::setup_sketch_defaults();init_ardupilot(); --->>> 3.1 分析// initialise the main loop scheduler --->>> 3.2 分析scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

3.1 init_ardupilot()

void Rover::init_ardupilot()
{// initialise console serial port--->>> 控制台串口初始化serial_manager.init_console();hal.console->printf("\n\nInit %s""\n\nFree RAM: %u\n",AP::fwversion().fw_string,(unsigned)hal.util->available_memory());//// Check the EEPROM format version before loading any parameters from EEPROM.//load_parameters(); --->>> 参数加载
#if STATS_ENABLED == ENABLED// initialise stats module --->>>统计信息模块g2.stats.init();
#endifmavlink_system.sysid = g.sysid_this_mav;// initialise serial ports --->>> 初始化所有的串口，根据不同的串口用途，进行对应的初始化，比如安照subs，gps，等串口对应的协议配置serial_manager.init();// setup first port early to allow BoardConfig to report errors--->>> 启动地面控制站终端gcs().setup_console();// Register mavlink_delay_cb, which will run anytime you have// more than 5ms remaining in your call to hal.scheduler->delay--->>> 注册mavlink_delay_cb，在调用hal.scheduler->delay剩余时间大于5ms的任意时间执行hal.scheduler->register_delay_callback(mavlink_delay_cb_static, 5);BoardConfig.init(); --->>> 板子相关，gpio，rc_in，rc_out，subs，uart等初始化
#if HAL_WITH_UAVCANBoardConfig_CAN.init(); --->>> can 初始化
#endif// init gripper 爪子初始化
#if GRIPPER_ENABLED == ENABLEDg2.gripper.init();
#endifg2.fence.init(); --->>> 围栏子系统初始化// initialise notify system --->>> 通知设备子系统初始化notify.init();notify_mode(control_mode);battery.init(); --->>> 电池监测子系统初始化// Initialise RPM sensor --->>> 转速传感器初始化rpm_sensor.init();rssi.init(); --->>> 信号接收强度初始化g2.airspeed.init(); --->>> 空速计初始化g2.windvane.init(serial_manager); --->>> 风标初始化// init baro before we start the GCS, so that the CLI baro test works--->>> 气压计初始化barometer.init();// setup telem slots with serial ports --->>> 地面控制站数传gcs().setup_uarts();#if OSD_ENABLED == ENABLEDosd.init(); --->>> 屏显
#endif#if LOGGING_ENABLED == ENABLEDlog_init(); --->>> 日志子系统初始化
#endif// initialise compass --->>> 指南针初始化AP::compass().set_log_bit(MASK_LOG_COMPASS);AP::compass().init();// initialise rangefinder --->>> 测距仪初始化rangefinder.init(ROTATION_NONE);// init proximity sensor --->>> 接近传感器初始化init_proximity();// init beacons used for non-gps position estimationinit_beacon(); --->>> 信标初始化，用于无gps情况下的位置估计// init visual odometryinit_visual_odom(); --->>> 虚拟里程计// and baro for EKF --->>> 气压计标定barometer.set_log_baro_bit(MASK_LOG_IMU);barometer.calibrate();// Do GPS init --->>> gps 初始化gps.set_log_gps_bit(MASK_LOG_GPS); gps.init(serial_manager);ins.set_log_raw_bit(MASK_LOG_IMU_RAW);--->>> 遥控器通道设置set_control_channels();  // setup radio channels and outputs rangesinit_rc_in();            // sets up rc channels deadzone 遥控器通道死区设置--->>> 马达初始化，包括设置伺服输出通道范围g2.motors.init();        // init motors including setting servo out channels rangesSRV_Channels::enable_aux_servos(); --->>> 使能辅助伺服通道// init wheel encoders --->>> 初始化轮子编码器g2.wheel_encoder.init();relay.init(); --->>> 中继初始化#if MOUNT == ENABLED// initialise camera mount 相机云台初始化camera_mount.init();
#endif/*setup the 'main loop is dead' check. Note that this relies onthe RC library being initialised.--->>> 失速检测定时器，检测“主循环已死”*/hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);// initialize SmartRTL --->>> 智能返航g2.smart_rtl.init();// initialise object avoidance --->>> 目标规避g2.oa.init();startup_ground(); --->>> 地面启动期间完成所有标定--->>> 设置模式为初始化模式Mode *initial_mode = mode_from_mode_num((enum Mode::Number)g.initial_mode.get());if (initial_mode == nullptr) {initial_mode = &mode_initializing;}set_mode(*initial_mode, ModeReason::INITIALISED);// initialise rc channels --->>> 初始化遥控器通道rc().init();rover.g2.sailboat.init(); --->>> 没帆呀？// disable safety if requested --->>> 保险开关BoardConfig.init_safety();// flag that initialisation has completedinitialised = true; --->>> 初始化完成标志#if AP_PARAM_KEY_DUMPAP_Param::show_all(hal.console, true); --->>> 打印所有参数变量的值
#endif
}

3.2 scheduler.init()

scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM); —>>> 传入调度任务表

FUNCTOR_TYPEDEF(task_fn_t, void); --->>> typedef Functor<void> task_fn_t
struct Task {task_fn_t function; --->>> Functor<void> function; 特化并构造函数对象const char *name;  --->>> 名称float rate_hz;       --->>> 调度频率uint16_t max_time_micros; --->>> 最大时间us
};#define SCHED_TASK(func, _interval_ticks, _max_time_micros) SCHED_TASK_CLASS(Rover, &rover, func, _interval_ticks, _max_time_micros)
/*useful macro for creating scheduler task table*/
#define SCHED_TASK_CLASS(classname, classptr, func, _rate_hz, _max_time_micros) { \.function = FUNCTOR_BIND(classptr, &classname::func, void),\AP_SCHEDULER_NAME_INITIALIZER(func)\.rate_hz = _rate_hz,\.max_time_micros = _max_time_micros\
}    #define FUNCTOR_BIND(obj, func, rettype, ...) \
Functor<rettype, ## __VA_ARGS__>::bind<std::remove_reference<decltype(*obj)>::type, func>(obj)
--->>> 调用模板类Functor的静态模板成员函数bind，decltype 编译时类型推导，返回表达式的类型，std::remove_reference移除引用类型返回原始类型template<class T, RetType (T::*method)(Args...)>
static constexpr Functor bind(T *obj)
{--->>> 以花括号初始化器列表初始化无名临时对象并返回return { obj, method_wrapper<T, method> }; --->>> 返回Functor 对象 method 即上面传递进来的 func
}
--->>> 看看上面初始化列表对应的构造方法：
constexpr Functor(void *obj, RetType (*method)(void *obj, Args...)): _obj(obj), _method(method) --->>> _method 赋值为 method_wrapper<T, method>
{
}
--->>> method_wrapper方法
template<class T, RetType (T::*method)(Args...)>
static RetType method_wrapper(void *obj, Args... args) --->>> 静态模板成员函数
{T *t = static_cast<T*>(obj); --->>> 具象return (t->*method)(args...); --->>> 成员函数指针调用
}
最后_method 赋值为 method_wrapper<T, method>
调用Functor 对象() 的形式即：
// Call the method on the obj this Functor is bound to --->>> 调用被该Functor绑定的对象上的方法
RetType operator()(Args... args) const
{return _method(_obj, args...);
}
就是调用特化静态模板成员 method_wrapper<T, method> 即 (t->*method)(args...); 调用对象obj的func方法
/*scheduler table - all regular tasks are listed here, along with howoften they should be called (in Hz) and the maximum timethey are expected to take (in microseconds)--->>> 列出了所有的定期任务，以及他们的调用频率，以及他们期望获得的最大时间(us)*/
const AP_Scheduler::Task Rover::scheduler_tasks[] = {//         Function name,          Hz,     us,--->>> 1. 读取遥控器 频率50Hz，一次花费200usSCHED_TASK(read_radio,             50,    200), --->>> --->>> 2. 航姿参考系统更新 频率400Hz，一次花费400us SCHED_TASK(ahrs_update,           400,    400),--->>> 3. 测距仪读取 频率50Hz，一次花费200usSCHED_TASK(read_rangefinders,      50,    200),--->>> 4. 更新当前模式 频率400Hz，一次花费200usSCHED_TASK(update_current_mode,   400,    200),--->>> 5. 设置伺服输出 频率400Hz，一次花费200usSCHED_TASK(set_servos,            400,    200),--->>> 6. 更新GPS 频率50Hz，一次花费300usSCHED_TASK(update_GPS,             50,    300),--->>> 7. 更新气压计 频率10Hz，一次花费200usSCHED_TASK_CLASS(AP_Baro,             &rover.barometer,        update,         10,  200),--->>> 8. 更新信标 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_Beacon,           &rover.g2.beacon,        update,         50,  200),--->>> 9. 更新接近传感器 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_Proximity,        &rover.g2.proximity,     update,         50,  200),--->>> 10. 更新风标 频率20Hz，一次花费100usSCHED_TASK_CLASS(AP_WindVane,         &rover.g2.windvane,      update,         20,  100),
#if VISUAL_ODOMETRY_ENABLED == ENABLED--->>> 11. 更新虚拟轮速传感器 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_VisualOdom,       &rover.g2.visual_odom,   update,         50,  200),
#endif--->>> 12. 更新围栏 频率10Hz，一次花费200usSCHED_TASK_CLASS(AC_Fence,            &rover.g2.fence,         update,         10,  100),--->>> 13. 更新轮子编码器 频率50Hz，一次花费200usSCHED_TASK(update_wheel_encoder,   50,    200),--->>> 14. 更新指南针 频率10Hz，一次花费200usSCHED_TASK(update_compass,         10,    200),--->>> 15. 更新任务 频率50Hz，一次花费200usSCHED_TASK(update_mission,         50,    200),--->>> 16. 更新日志记录1 频率10Hz，一次花费200usSCHED_TASK(update_logging1,        10,    200),--->>> 17. 更新日志记录2 频率10Hz，一次花费200usSCHED_TASK(update_logging2,        10,    200),--->>> 18. 更新从地面站接收的数据 频率400Hz，一次花费500usSCHED_TASK_CLASS(GCS,                 (GCS*)&rover._gcs,       update_receive,                    400,    500),--->>> 19. 更新向地面站发送数据 频率400Hz, 一次花费1000usSCHED_TASK_CLASS(GCS,                 (GCS*)&rover._gcs,       update_send,                       400,   1000),--->>> 20. 读取遥控器模式切换开关 频率7Hz，一次花费200usSCHED_TASK_CLASS(RC_Channels,         (RC_Channels*)&rover.g2.rc_channels, read_mode_switch,        7,    200),--->>> 21. 读取遥控器辅助通道 频率10Hz，一次花费200usSCHED_TASK_CLASS(RC_Channels,         (RC_Channels*)&rover.g2.rc_channels, read_aux_all,           10,    200),--->>> 22. 读取电池监测信息 频率10Hz，一次花费300usSCHED_TASK_CLASS(AP_BattMonitor,      &rover.battery,          read,           10,  300),--->>> 23. 更新伺服中继事件 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_ServoRelayEvents, &rover.ServoRelayEvents, update_events,  50,  200),
#if GRIPPER_ENABLED == ENABLED--->>> 24. 更新机械臂控制 频率10Hz，一次花费75usSCHED_TASK_CLASS(AP_Gripper,          &rover.g2.gripper,      update,         10,   75),
#endif--->>> 25. 更新转速 频率10Hz，一次花费100usSCHED_TASK(rpm_update,             10,    100),
#if MOUNT == ENABLED--->>> 26. 更新相机云台控制 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_Mount,            &rover.camera_mount,     update,         50,  200),
#endif
#if CAMERA == ENABLED--->>> 27. 更新摄像头事件 频率50Hz，一次花费200usSCHED_TASK_CLASS(AP_Camera,           &rover.camera,           update_trigger, 50,  200),
#endif--->>> 28. 地面站失速检测 频率10Hz，一次花费200usSCHED_TASK(gcs_failsafe_check,     10,    200),--->>> 29.围栏检测 频率10Hz，一次花费200usSCHED_TASK(fence_check,            10,    200),--->>> 30. EKF检测 频率10Hz，一次花费100usSCHED_TASK(ekf_check,              10,    100),--->>> 31. 智能返航模式位置存储 频率3Hz，一次花费200usSCHED_TASK_CLASS(ModeSmartRTL,        &rover.mode_smartrtl,    save_position,   3,  200),--->>> 32. 更新通知 频率50Hz，一次花费300usSCHED_TASK_CLASS(AP_Notify,           &rover.notify,           update,         50,  300),--->>> 33. 一秒循环 频率1Hz，一次花费1500usSCHED_TASK(one_second_loop,         1,   1500),--->>> 34. 喷雾 频率3Hz，一次花费90usSCHED_TASK_CLASS(AC_Sprayer,          &rover.g2.sprayer,           update,      3,  90),--->>> 35. 指南针校准 频率50Hz，一次花费200usSCHED_TASK_CLASS(Compass,          &rover.compass,              cal_update, 50, 200),--->>> 36. 指南针存储 频率0.1Hz，一次花费200usSCHED_TASK(compass_save,           0.1,   200),--->>> 37. 加速度计校准 频率10Hz，一次花费200usSCHED_TASK(accel_cal_update,       10,    200),
#if LOGGING_ENABLED == ENABLED--->>> 38. 日志周期任务 频率50Hz， 一次花费300usSCHED_TASK_CLASS(AP_Logger,     &rover.logger,        periodic_tasks, 50,  300),
#endif--->>> 39. 惯性传感器 频率400Hz，一次花费200usSCHED_TASK_CLASS(AP_InertialSensor,   &rover.ins,              periodic,      400,  200),--->>> 40. 调度器更新记录 频率0.1Hz, 一次花费200usSCHED_TASK_CLASS(AP_Scheduler,        &rover.scheduler,        update_logging, 0.1, 200),--->>> 41. 按键 频率5Hz，一次花费200usSCHED_TASK_CLASS(AP_Button,           &rover.button,           update,          5,  200),
#if STATS_ENABLED == ENABLED--->>> 42. 状态更新 频率5Hz， 一次花费200usSCHED_TASK(stats_update,            1,    200),
#endif--->>> 43. 崩溃检测 频率10Hz，一次花费200usSCHED_TASK(crash_check,            10,    200),--->>> 44. 巡航 频率50Hz，一次花费200usSCHED_TASK(cruise_learn_update,    50,    200),
#if ADVANCED_FAILSAFE == ENABLED--->>> 45. 高级失速保护 频率10Hz，一次花费200usSCHED_TASK(afs_fs_check,           10,    200),
#endif--->>> 46. 读取空速计 频率10Hz，一次花费100usSCHED_TASK(read_airspeed,          10,    100),
#if OSD_ENABLED == ENABLED--->>> 47. 发布屏显信息 频率1Hz, 一次花费10usSCHED_TASK(publish_osd_info,        1,     10),
#endif
};

目前共计47个任务

// initialise the scheduler
void AP_Scheduler::init(const AP_Scheduler::Task *tasks, uint8_t num_tasks, uint32_t log_performance_bit)
{_tasks = tasks; --->>> 承载任务表的首地址_num_tasks = num_tasks; --->>> 表项个数// tick counter at the time we last ran each task_last_run = new uint16_t[_num_tasks]; --->>> 最后一次运行该任务时的tick值记录memset(_last_run, 0, sizeof(_last_run[0]) * _num_tasks);_tick_counter = 0; --->>> tick 计数器归零// setup initial performance counters --->>> 设置初始执行计数器perf_info.set_loop_rate(get_loop_rate_hz()); --->>> 循环频率perf_info.reset();_log_performance_bit = log_performance_bit;
}

4. 主循环：

void loop(void) override;

void Rover::loop()
{scheduler.loop();G_Dt = scheduler.get_last_loop_time_s();
}void AP_Scheduler::loop()
{// wait for an INS sample --->>> 等待INS采样就绪hal.util->persistent_data.scheduler_task = -3;AP::ins().wait_for_sample();hal.util->persistent_data.scheduler_task = -1;const uint32_t sample_time_us = AP_HAL::micros(); --->>> 记录采样完成的时刻if (_loop_timer_start_us == 0) { --->>> 如果定时循环开始时刻为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) {hal.util->persistent_data.scheduler_task = -2;_fastloop_fn();hal.util->persistent_data.scheduler_task = -1;}#if CONFIG_HAL_BOARD == HAL_BOARD_SITL{/*for testing low CPU conditions we can add an optional delay in SITL*/auto *sitl = AP::sitl();uint32_t loop_delay_us = sitl->loop_delay.get();hal.scheduler->delay_microseconds(loop_delay_us);}
#endif// tell the scheduler one tick has passed --->>>经过一个 ticktick();// 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--->>> 运行所有应该运行的任务。我们每次循环只调用一次，尽管任务们被主循环tick调度多次。因此如果他们在首次调用调度器时不执行，他们此后也不执行，直到调度器进入下次tick <<<---const uint32_t loop_us = get_loop_period_us(); --->>> 获取给予每次循环的时间uint32_t now = AP_HAL::micros(); --->>> 获取当前时刻uint32_t time_available = 0;if (now - sample_time_us < loop_us) { --->>> 如果当前时刻 - 采样完成时刻 < 给予每次循环的时间// get remaining time available for this loop --->>> 获取此次循环的可用时间 = 给予每次循环时间 - (当前时刻 - 采样完成时刻), 后两个相减是上面采样完成后到当前 这段代码的总执行时间       time_available = loop_us - (now - sample_time_us);}// add in extra loop time determined by not achieving scheduler tasks --->>> 加上由未完成的调度任务决定的额外循环时间(初次为0)time_available += extra_loop_us;// run the tasks --->>> 运行任务们come on！ 传入此次循环的可用时间 ，5 小节 分析run(time_available);#if CONFIG_HAL_BOARD == HAL_BOARD_SITL// move result of AP_HAL::micros() forward:hal.scheduler->delay_microseconds(1);
#endifif (task_not_achieved > 0) { --->>> 存在未达成目标的任务: 即在该任务期望的间隔tick数后，该任务没有按时得到执行// add some extra time to the budget --->>> 增加时间预算extra_loop_us = MIN(extra_loop_us+100U, 5000U); --->>> 100 - 5000之间的较小值(us)，每次递增100us，5ms为上线task_not_achieved = 0;task_all_achieved = 0;} else if (extra_loop_us > 0) { --->>> 不存在未达成目标的任务，但额外时间非0task_all_achieved++; --->>> 所有任务按预期都达成目标计数自增if (task_all_achieved > 50) {// we have gone through 50 loops without a task taking too// long. CPU pressure has eased, so drop the extra time we're// giving each loop --->>> 我们已经进行了没有任务消耗超长时间的50次循环，task_all_achieved = 0;// we are achieving all tasks, slowly lower the extra loop time --->>> 我们实现了所有任务目标，缓慢降低额外循环时间，每次递减50usextra_loop_us = MAX(0U, extra_loop_us-50U);}}// check loop time // --->>> 检查循环时间：采样完成时刻 - 循环开始时刻，首次运行 时相减为0// --->>> 再次loop到此时：sample_time_us 为新一轮的采样完成时刻，而 _loop_timer_start_us 为上一轮的循环开始时刻// --->>> 相当于得到了两轮之间的时间差perf_info.check_loop_time(sample_time_us - _loop_timer_start_us); _loop_timer_start_us = sample_time_us; --->>> 更新循环开始时刻 为 此轮的采样完成时刻
}

5. run 方法

void AP_Scheduler::run(uint32_t time_available)

/*run one tickthis will run as many scheduler tasks as we can in the specified time--->>> 运行一个tick循环, 在规定时间内尽可能多地运行调度器任务*/
void AP_Scheduler::run(uint32_t time_available)
{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 (uint8_t i=0; i<_num_tasks; i++) { --->>> 每tick一次，都遍历任务调度表uint32_t dt = _tick_counter - _last_run[i]; --->>> dt = 当前的_tick_counter - 最后一次运行该任务的_tick_counter，即：距上次运行该任务后经过的tick数///--->>> 调度器循环频率意味着 tick的频率，就Rover而言 tick 频率50Hz，即20ms 一次自增///--->>> 间隔的tick数 = 50Hz / (任务运行频率，比如 400Hz， 或者10Hz，等等任务调度表中指定的任务运行频率) ///--->>> interval_ticks = 50/400 -> 1/8 或者50/10 = 5///--->>> 如此所谓间隔tick数_interval_ticks 也就是: 该任务在当前特定的循环频率(tick频率)下，需要间隔几个tick就轮到该任务执行///--->>> 比如上面的任务频率400Hz得到interval_tick= 1/8 也就是 每0.125个tick就需要执行该任务了,因为tick是以1为最小步进，所以经过一个tick，该任务就必须投入执行///--->>> 同理: 任务频率10Hz，得到interval_tick=5，也就是每5个tick就需要执行该任务了uint32_t interval_ticks = _loop_rate_hz / _tasks[i].rate_hz; --->>> 间隔的tick数 = (调度器loop循环频率)50Hz / 该任务的运行频率if (interval_ticks < 1) { --->>> 间隔tick数小于1, 说明每次loop循环tick，该任务都需要执行interval_ticks = 1; --->>> 取整到一个tick步长}if (dt < interval_ticks) { --->>> 如果tick之差 小于该任务的间隔tick数，则该任务不需要执行// 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) { --->>> 如果tick之差 >= 两倍的任务间隔tick数，必然出现了异常// we've slipped a whole run of this task!--->>> 报bug，我们忽略了该任务的一次完整运行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 (dt >= interval_ticks*max_task_slowdown) { --->>>  距上次运行该任务后到本次运行该任务所经过的tick数 >= 任务间隔tick数 * 最大的任务减速因子(默认4)// we are going beyond the maximum slowdown factor for a --->>> 我们已经超出了任务的最大减速因子// task. This will trigger increasing the time budget --->>> 这将会引起时间预算增加//--->>> 也就是，当前求得距上次运行该任务进过的实际tick数 >= 任务的理论间隔tick数(任务表中指定) * 最大任务减速因子//--->>> 即: 实际任务间隔tick数 >= 理论任务间隔tick数 * 最大任务减速因子task_not_achieved++; --->>> 未达成目标计数}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;--->>> 记录该任务开始运行的时刻hal.util->persistent_data.scheduler_task = i;if (_debug > 1 && _perf_counters && _perf_counters[i]) {hal.util->perf_begin(_perf_counters[i]);}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITLfill_nanf_stack();
#endif_tasks[i].function(); --->>> 运行任务的函数对象，进入-运行-退出if (_debug > 1 && _perf_counters && _perf_counters[i]) {hal.util->perf_end(_perf_counters[i]);}hal.util->persistent_data.scheduler_task = -1;// record the tick counter when we ran. This drives// when we next run the event --->>> 记录此次运行的_tick_counter值，这将成为我们下次运行该项的驱动力_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) { --->>> 如果实际时间 > 该任务最大期望花费时间，报bug// 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) { --->>> 如果该任务实际花费时间 >= tick循环的可用时间，说明此次tick的时间片已经耗尽，退出任务列表的遍历执行time_available = 0; --->>> 这种情况下清零此次tick循环可用时间break;}time_available -= time_taken; --->>> 更新此次tick循环可用时间(减去该任务实际花费时间后的余量)}// update number of spare microseconds --->>> 剩余时间更新_spare_micros += time_available;_spare_ticks++; --->>> 剩余时间tick数自增if (_spare_ticks == 32) { --->>> 每32次 ??? ???_spare_ticks /= 2; --->>> 剩余时间tick数折半_spare_micros /= 2; --->>> 剩余时间折半}
}

总结

首先AP_Vehicle纯抽象载具派生自AP_HAL::HAL::Callbacks（提供框架接口）, Rover载具派生自AP_Vehicle，并实现了期望的行为setup和loop；
在stm32启动后，进入main方法中，已被从数据段加载到内存中的全局Rover实例到安照前面分析过得启动框架调用setup进行各种初始化，其中就包括初始化调度器对象，传入函数对象表征的待运行的目前最多47个Rover任务列表；
Rover初始化完成后，启动框架调用Rover::loop，最终进入调度器loop，采样INS数据，遍历任务列表执行所有任务；

APM_Rover运行纲领分析，以pixhawk-fmuv2为硬件平台，ChibiOS为底层操作系统:相关推荐

AP_HAL 分析, 以pixhawk-fmuv2为硬件平台，ChibiOS为底层操作系统:
1. class AP_HAL::AP_HAL,该接口类聚合了所有提供给应用层的硬件接口 class AP_HAL::HAL { public:HAL(AP_HAL::UARTDriver* _uar ...
【Android 逆向】GDA 逆向工具安装 ( GDA 下载 | GDA 简介 | 运行 GDA 分析 APK 文件 )
文章目录一.GDA 相关资料二.运行 GDA 分析 APK 文件一.GDA 相关资料 GDA 相关资料 : GDA 工具官网 : http://www.gda.wiki:9090 GDA 文档 ...
全球及中国原料药产业投资动态及未来运行前景分析报告2021-2027年
全球及中国原料药产业投资动态及未来运行前景分析报告2021-2027年 HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS--HS-- ...
Spark on YARN cluster client 模式作业运行全过程分析
一.Spark:Yarn-Cluster 与 Yarn-Client 的区别与联系我们都知道Spark支持在yarn上运行,但是Spark on yarn有分为两种模式yarn-cluster和ya ...
《容器技术系列》一1.4　Docker运行案例分析
本节书摘来华章计算机<容器技术系列>一书中的第1章 ,第1.4节,孙宏亮著, 更多章节内容可以访问云栖社区"华章计算机"公众号查看. 1.4 Docker运行案例分析 ...
java启动servlet_Java Servlet 运行原理分析
1 Servlet基本执行过程 Web容器(如Tomcat)判断当前请求是否第一次请求Servlet程序 . 如果是第一次,则Web容器执行以下任务: 加载Servlet类. 实例化Servlet类. ...
3516a 自带的ive 算子的运行情况分析
3516a 自带的ive 算子的运行情况分析 3516a 自带的ive sample 可以成功编译过 3516a 有8个函数的demo: 除了2 3 4 功能运行不成功之外其他的功能都可以用. 实际 ...
大数据该如何运行与分析
大数据时代是一个时代的象征,也是一个改变人们的生活的一个常态.大数据不只是分析数据的一个时代,更是方便人们选择的一种个数据分析.例如今天我们说看到的产品,大数据会通过分析你的需求,为您推送更好的产品, ...
2022版中国科技中介服务产业运行现状分析与发展机遇规划报告
2022版中国科技中介服务产业运行现状分析与发展机遇规划报告 ------------------------------------- <修订日期>:2022年2月 <出版单位& ...
uIP TCP Server 运行机制分析
uIP TCP Server 运行机制分析 DanielLee_USTB 2013.9.22 对于任何一个协议栈而言,首先要实现的就是TCP Server,下面看看uIP的运行机制: ...