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 bus
    AP_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
APM_Rover运行纲领分析,以pixhawk-fmuv2为硬件平台,ChibiOS为底层操作系统:_第1张图片

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 == ENABLED
    friend class AP_AdvancedFailsafe_Rover;
#endif
    friend 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 parameters
    Parameters 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 array
    AP_Int8 *modes;
    const uint8_t num_modes = 6;

    // AP_RPM Module --->>> 转速模块
    AP_RPM rpm_sensor;

    // Inertial Navigation EKF
#if AP_AHRS_NAVEKF_AVAILABLE
    NavEKF2 EKF2{&ahrs, rangefinder};
    NavEKF3 EKF3{&ahrs, rangefinder};
    AP_AHRS_NavEKF ahrs{EKF2, EKF3};
#else
    AP_AHRS_DCM ahrs;
#endif

    // Arming/Disarming management class
    AP_Arming_Rover arming;

    AP_L1_Control L1_controller{ahrs, nullptr}; --->>> L1 辅助驾驶

#if AP_AHRS_NAVEKF_AVAILABLE
    OpticalFlow optflow;
#endif

#if OSD_ENABLED == ENABLED
    AP_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 location
    struct Location current_loc;

    // Camera
#if CAMERA == ENABLED
    AP_Camera camera{MASK_LOG_CAMERA, current_loc};
#endif

    // Camera/Antenna mount tracking and stabilisation stuff
#if MOUNT == ENABLED
    AP_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 switch
    uint8_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 failsafe
        uint8_t triggered;          // bit flags of failsafes that have triggered an action
        uint32_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/s
    float 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 running
    uint32_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.cpp
    void 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.cpp
    void 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 == ENABLED
    void 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 == ENABLED
    void publish_osd_info(); --->>> 发布屏显信息
#endif

    enum 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 test
    void 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 mode
    float 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();
#endif

    mavlink_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_UAVCAN
    BoardConfig_CAN.init(); --->>> can 初始化
#endif

    // init gripper 爪子初始化
#if GRIPPER_ENABLED == ENABLED
    g2.gripper.init();
#endif

    g2.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 == ENABLED
    osd.init(); --->>> 屏显
#endif

#if LOGGING_ENABLED == ENABLED
    log_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 estimation
    init_beacon(); --->>> 信标初始化,用于无gps情况下的位置估计

    // init visual odometry
    init_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 ranges
    init_rc_in();            // sets up rc channels deadzone 遥控器通道死区设置
    --->>> 马达初始化,包括设置伺服输出通道范围
    g2.motors.init();        // init motors including setting servo out channels ranges
    SRV_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 on
      the 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 completed
    initialised = true; --->>> 初始化完成标志

#if AP_PARAM_KEY_DUMP
    AP_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 task_fn_t
struct Task {
    task_fn_t function; --->>> Functor 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::bind::type, func>(obj)
--->>> 调用模板类Functor的静态模板成员函数bind,decltype 编译时类型推导,返回表达式的类型,std::remove_reference移除引用类型返回原始类型

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

    if (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) { --->>> 不存在未达成目标的任务,但额外时间非0
        task_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 --->>> 我们实现了所有任务目标,缓慢降低额外循环时间,每次递减50us
            extra_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 tick
  this 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_SITL
        fill_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数据,遍历任务列表执行所有任务;

你可能感兴趣的:(Ardupilot,源码剖析)