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();
};
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 == 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;
};
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);
}
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
}
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;
}
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; --->>> 更新循环开始时刻 为 此轮的采样完成时刻
}
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; --->>> 剩余时间折半
}
}