一、前言
本文就主要来讲一讲APM里面的车辆类型好了，当然是以Sub作为主要对象，不过前面的继承关系和基类基本都是一致的，也可以作为参考。
Sub类定义在 Sub.h 文件内部，其向上继承自 AP_Vechile 类，这是所有无人机车辆类型（Copter、Rover…）的父类。同样，AP_Vechile向上也有相应的继承关系，具体来说，用下面这种方式表达：
AP_HAL::HAL::Callbacks
|---- AP_Vechile
|---- Sub
因此，为了能对Sub类的车辆类型进行深入了解，我们还是得回到最源头讲起。
二、class AP_HAL::HAL
该类定义于 libraries/AP_HAL/HAL.h 路径下，命名空间为AP_HAL（具体查看 AP_HAL_Namespace.h），该类内部由各种最基础的底层外设抽象组合而成，构成了一个HAL抽象基类组件的集合。
class AP_HAL::HAL {
public:
HAL(AP_HAL::UARTDriver* _uartA, // console //各基本外设抽象基类
AP_HAL::UARTDriver* _uartB, // 1st GPS
AP_HAL::UARTDriver* _uartC, // telem1
AP_HAL::UARTDriver* _uartD, // telem2
AP_HAL::UARTDriver* _uartE, // 2nd GPS
AP_HAL::UARTDriver* _uartF, // extra1
AP_HAL::UARTDriver* _uartG, // extra2
AP_HAL::UARTDriver* _uartH, // extra3
AP_HAL::I2CDeviceManager* _i2c_mgr,
AP_HAL::SPIDeviceManager* _spi,
AP_HAL::AnalogIn* _analogin,
AP_HAL::Storage* _storage,
AP_HAL::UARTDriver* _console,
AP_HAL::GPIO* _gpio,
AP_HAL::RCInput* _rcin,
AP_HAL::RCOutput* _rcout,
AP_HAL::Scheduler* _scheduler,
AP_HAL::Util* _util,
AP_HAL::OpticalFlow*_opticalflow,
AP_HAL::Flash* _flash,
AP_HAL::DSP* _dsp,
#if HAL_NUM_CAN_IFACES > 0 //CAN设备管理
AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES])
#else
AP_HAL::CANIface** _can_ifaces)
#endif
:
uartA(_uartA), //初始化列表，基类指向子类
uartB(_uartB),
uartC(_uartC),
uartD(_uartD),
uartE(_uartE),
uartF(_uartF),
uartG(_uartG),
uartH(_uartH),
i2c_mgr(_i2c_mgr),
spi(_spi),
analogin(_analogin),
storage(_storage),
console(_console),
gpio(_gpio),
rcin(_rcin),
rcout(_rcout),
scheduler(_scheduler),
util(_util),
opticalflow(_opticalflow),
flash(_flash),
dsp(_dsp)
{
#if HAL_NUM_CAN_IFACES > 0 //CAN管理
if (_can_ifaces == nullptr) {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
can[i] = nullptr;
} else {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
can[i] = _can_ifaces[i];
}
#endif
AP_HAL::init();
}
struct Callbacks { //基础抽象类组件，内部纯虚函数等待子类重写
virtual void setup() = 0;
virtual void loop() = 0;
};
struct FunCallbacks : public Callbacks { //派生自Callbacks组件
FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void));
void setup() override { _setup(); }
void loop() override { _loop(); }
private:
void (*_setup)(void);
void (*_loop)(void);
};
//run接口方法
virtual void run(int argc, char * const argv[], Callbacks* callbacks) const = 0;
//抽象类基础成员组件
AP_HAL::UARTDriver* uartA;
AP_HAL::UARTDriver* uartB;
AP_HAL::UARTDriver* uartC;
AP_HAL::UARTDriver* uartD;
AP_HAL::UARTDriver* uartE;
AP_HAL::UARTDriver* uartF;
AP_HAL::UARTDriver* uartG;
AP_HAL::UARTDriver* uartH;
AP_HAL::I2CDeviceManager* i2c_mgr;
AP_HAL::SPIDeviceManager* spi;
AP_HAL::AnalogIn* analogin;
AP_HAL::Storage* storage;
AP_HAL::UARTDriver* console;
AP_HAL::GPIO* gpio;
AP_HAL::RCInput* rcin;
AP_HAL::RCOutput* rcout;
AP_HAL::Scheduler* scheduler;
AP_HAL::Util *util;
AP_HAL::OpticalFlow *opticalflow;
AP_HAL::Flash *flash;
AP_HAL::DSP *dsp;
#if HAL_NUM_CAN_IFACES > 0
AP_HAL::CANIface* can[HAL_NUM_CAN_IFACES];
#else
AP_HAL::CANIface** can;
#endif
};
参考：Ardupilot之cpu外设基础抽象聚合类 HAL.h
三、class AP_Vehicle
3.1 .h
该类定义于 libraries/AP_Vehicle/AP_Vehicle.h，派生自AP_HAL::HAL::Callbacks接口类，因此拥有 setup() 和 loop() 两种行为。该类的主要目的就是作为各种具体车辆类型的基类，提供最基本的功能，并在具象化特定车辆时提供相应的接口。
代码有点长，一些必要的地方已经备注，请大家耐心看完
class AP_Vehicle : public AP_HAL::HAL::Callbacks {
public:
// 构造方法以确保单例
AP_Vehicle() {
if (_singleton) {
AP_HAL::panic("Too many Vehicles");
}
AP_Param::setup_object_defaults(this, var_info);
_singleton = this;
}
// 禁止拷贝构造方法防止递归引用
AP_Vehicle(const AP_Vehicle &other) = delete;
AP_Vehicle &operator=(const AP_Vehicle&) = delete;
static AP_Vehicle *get_singleton();
// 注意此处同时使用了override和final关键字
// 表示函数可以覆盖来自基类AP_HAL::HAL::Callbacks的函数，但是禁止在派生类中重写该方法
// 但是可以在init_ardupilot中进行特定于子类的初始化，此初始化是从setup()调用
// setup（）在车辆启动期间被调用一次，以初始化车辆对象及其包含的对象
// AP_HAL_MAIN_CALLBACKS编译指示创建一个main(...)函数，该函数引用包含setup()和loop()函数的对象
void setup(void) override final;
void loop() override final;
// 纯虚函数提供接口，希望子类重写set_mode()和get_mode()方法
bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0;
uint8_t virtual get_mode() const = 0;
/*
固定翼飞行的通用参数
*/
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; // roll和pitch上下限
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;
};
// 飞行状态
enum FlightStage {
FLIGHT_TAKEOFF = 1, // 起飞
FLIGHT_VTOL = 2, // 垂直起降
FLIGHT_NORMAL = 3, // 正常飞行
FLIGHT_LAND = 4, // 着陆
FLIGHT_ABORT_LAND = 7 // 终止着陆
};
};
/*
多旋翼通用参数
*/
struct MultiCopter {
AP_Int16 angle_max; // 最大角度
};
//获取任务列表及任务数
void get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks);
// implementations *MUST* fill in all passed-in fields or we get
// Valgrind errors
virtual void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) = 0;
/*
设置是否处于飞行标志，无法保证准确度，但是是对于无人机是否处于飞行状态的最佳估计
*/
void set_likely_flying(bool b) {
if (b && !likely_flying) {
_last_flying_ms = AP_HAL::millis();
}
likely_flying = b;
}
/*
获取飞行状态，返回值为true则表明处于飞行中，无法保证准确性
*/
bool get_likely_flying(void) const {
return likely_flying;
}
/*
返回以ms为单位的时间，开始时间为likely_flying被设置为true
如果likely_flying为false，则返回0
*/
uint32_t get_time_flying_ms(void) const {
if (!likely_flying) {
return 0;
}
return AP_HAL::millis() - _last_flying_ms;
}
// 如果车辆发生碰撞，返回true
virtual bool is_crashed() const;
/*
通过脚本控制车辆使用的函数
*/
virtual bool start_takeoff(float alt) { return false; }
virtual bool set_target_location(const Location& target_loc) { return false; }
virtual bool set_target_velocity_NED(const Vector3f& vel_ned) { return false; }
virtual bool set_target_angle_and_climbrate(float roll_deg, float pitch_deg, float yaw_deg, float climb_rate_ms, bool use_yaw_rate, float yaw_rate_degs) { return false; }
// 获取目标位置（供脚本使用）
virtual bool get_target_location(Location& target_loc) { return false; }
// 设置转向和油门（-1到+1）（供Rover脚本使用）
virtual bool set_steering_and_throttle(float steering, float throttle) { return false; }
// 控制输出量的枚举类型
enum class ControlOutput {
Roll = 1,
Pitch = 2,
Throttle = 3,
Yaw = 4,
Lateral = 5,
MainSail = 6,
WingSail = 7,
Walking_Height = 8,
Last_ControlOutput // place new values before this
};
// 获取控制输出（用于脚本）
// 成功返回true，并且control_value设置为-1到+1范围内的值
virtual bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) { return false; }
// 输出谐波陷波日志消息
void write_notch_log_messages() const;
// 更新谐波陷波
virtual void update_dynamic_notch() {};
protected:
virtual void init_ardupilot() = 0;
virtual void load_parameters() = 0;
virtual void set_control_channels() {}
// 板级特定配置
AP_BoardConfig BoardConfig;
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
// 对于CAN总线的配置
AP_CANManager can_mgr;
#endif
// 主循环调度器
AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&AP_Vehicle::fast_loop, void)};
virtual void fast_loop();
// IMU 变量
// 积分时间; 最后一次循环运行的时间
float G_Dt;
// 传感器定义
AP_GPS gps;
AP_Baro barometer;
Compass compass;
AP_InertialSensor ins;
AP_Button button;
RangeFinder rangefinder;
AP_RSSI rssi;
#if HAL_RUNCAM_ENABLED
AP_RunCam runcam;
#endif
#if HAL_GYROFFT_ENABLED
AP_GyroFFT gyro_fft;
#endif
AP_VideoTX vtx;
AP_SerialManager serial_manager;
AP_Relay relay;
AP_ServoRelayEvents ServoRelayEvents;
// LED，蜂鸣器等的通知对象（参数设置为false将禁用外部LED）
AP_Notify notify;
// 扩展卡尔曼滤波的惯性导航
#if AP_AHRS_NAVEKF_AVAILABLE
AP_AHRS_NavEKF ahrs;
#else
AP_AHRS_DCM ahrs;
#endif
#if HAL_HOTT_TELEM_ENABLED
AP_Hott_Telem hott_telem;
#endif
#if HAL_VISUALODOM_ENABLED
AP_VisualOdom visual_odom;
#endif
AP_ESC_Telem esc_telem;
#if HAL_MSP_ENABLED
AP_MSP msp;
#endif
#if GENERATOR_ENABLED
AP_Generator_RichenPower generator;
#endif
static const struct AP_Param::GroupInfo var_info[];
static const struct AP_Scheduler::Task scheduler_tasks[];
private:
// delay() 回调处理MAVLink数据包
static void scheduler_delay_callback();
// 如果有看门狗重置，请通过statustext通知
void send_watchdog_reset_statustext();
bool likely_flying; // 车辆被判断为可能处于飞行的状态时将会被设置为true
uint32_t _last_flying_ms; // 上一次likely_flying被设置为true的时间
static AP_Vehicle *_singleton;
};
需要注意的是在公有部分内部声明的两个函数setup()和loop()。之前查了很多博客，包括截至写博客期间的官方手册，讲述的都是以前的版本。以前版本的APM源码在对应的具体车辆类型中（如Copter等）是在主文件中（如Copter.cpp）中通过setup()完成初始化，loop()完成主循环。现在的话主循环完成在fast_loop()里面。具体内容我打算另写一篇博文细讲一下，这边就先点到为止。
// 注意此处同时使用了override和final关键字
// 表示函数可以覆盖来自基类AP_HAL::HAL::Callbacks的函数，但是禁止在派生类中重写该方法
// 但是可以在init_ardupilot中进行特定于子类的初始化，此初始化是从setup()调用
// setup（）在车辆启动期间被调用一次，以初始化车辆对象及其包含的对象
// AP_HAL_MAIN_CALLBACKS编译指示创建一个main(...)函数，该函数引用包含setup()和loop()函数的对象
void setup(void) override final;
void loop() override final;
此外在protected部分，需要注意的是init_ardupilot()和fast_loop()两个方法。
virtual void init_ardupilot() = 0;
...
virtual void fast_loop();
3.2 .cpp
在对应的AP_Vechile.cpp文件中实现了setup()，loop()和fast_loop()函数。
/*
setup is called when the sketch starts
*/
void AP_Vehicle::setup()
{
// 加载变量表var_info[]中变量的默认值
AP_Param::setup_sketch_defaults();
// 初始化串口
serial_manager.init_console();
hal.console->printf("\n\nInit %s"
"\n\nFree RAM: %u\n",
AP::fwversion().fw_string,
(unsigned)hal.util->available_memory());
load_parameters();
// 初始化主循环任务
const AP_Scheduler::Task *tasks;
uint8_t task_count;
uint32_t log_bit;
get_scheduler_tasks(tasks, task_count, log_bit);
AP::scheduler().init(tasks, task_count, log_bit);
// 每个循环的时间-根据实际的循环速率在main loop（）中进行更新
G_Dt = scheduler.get_loop_period_s();
// this is here for Plane; its failsafe_check method requires the
// RC channels to be set as early as possible for maximum
// survivability.
set_control_channels();
// 在启动过程中尽早初始化串行管理器以获取诊断输出。 我们必须首先初始化GCS单例，因为它设置了可能在早期使用的全局mavlink系统ID。
gcs().init();
// 初始化串口设备
serial_manager.init();
gcs().setup_console();
// 注册scheduler_delay_cb，它将在您对hal.scheduler-> delay的调用中剩余超过5毫秒的任何时间运行
hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
#if HAL_MSP_ENABLED
// 在init_ardupilot之前调用MSP init以允许MSP传感器
msp.init();
#endif
// 这部分函数是车辆完成大部分初始化的地方
init_ardupilot();
// gyro FFT 需要在较晚的时间完成初始化
#if HAL_GYROFFT_ENABLED
gyro_fft.init(AP::scheduler().get_loop_period_us());
#endif
#if HAL_RUNCAM_ENABLED
runcam.init();
#endif
#if HAL_HOTT_TELEM_ENABLED
hott_telem.init();
#endif
#if HAL_VISUALODOM_ENABLED
// 用于视觉位置估计的初始化库
visual_odom.init();
#endif
vtx.init();
#if AP_PARAM_KEY_DUMP
AP_Param::show_all(hal.console, true);
#endif
}
void AP_Vehicle::loop()
{
scheduler.loop();
G_Dt = scheduler.get_loop_period_s();
}
/*
所有车辆的快速循环回调。 这将在任何特定于车辆的快速循环结束时调用。
*/
void AP_Vehicle::fast_loop()
{
#if HAL_GYROFFT_ENABLED
gyro_fft.sample_gyros();
#endif
}
总结：
- 在setup()里面完成了如串口和参数表等部分设备的初始化内容，但是具体的车辆设备初始化需要在init_ardupilot()中完成。
- loop()函数较短，内部实现的是对任务循环并且获取任务单词循环时的时间（以s为单位）。
- fast_loop()见注释。
四、class Sub
4.1 .h
内容实在是有点多，为了排版和阅读方便，这边就不全部放上来了，挑一些重点说一下。
public:
friend class GCS_MAVLINK_Sub;
friend class GCS_Sub;
friend class Parameters;
friend class ParametersG2;
friend class AP_Arming_Sub;
friend class RC_Channels_Sub;
Sub(void);
首先是公有部分，内部声明了6个友元类，这些类中的成员函数能够访问Sub类中的私有成员。构造函数Sub()在Sub.cpp文件中实现。
// primary input control channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
RC_Channel *channel_forward;
RC_Channel *channel_lateral;
这部分上一篇博客已经提过了，但是这边还是想说一下，此处表示的是Sub的主要输入控制通道，一个通道可能会对应多个电机（具体关系请看我上一篇博文）。
// 失效保护
struct {
uint32_t last_leak_warn_ms; // 上次向gcs发送泄漏警告的时间
uint32_t last_gcs_warn_ms;
uint32_t last_heartbeat_ms; // 从GCS到达最后一个HEARTBEAT消息的时间-用于触发gcs故障保护的时间
uint32_t last_pilot_input_ms; // 上次我们以MANUAL_CONTROL或RC_CHANNELS_OVERRIDE消息的形式收到飞行员输入时间
uint32_t terrain_first_failure_ms; // 第一次地形数据访问失败-用于计算失败的持续时间
uint32_t terrain_last_failure_ms; // 最近一次地形数据访问失败
uint32_t last_crash_warn_ms; // 上次向gcs发送崩溃警告
uint32_t last_ekf_warn_ms; // 上一次向gcs发送ekf警告
uint8_t pilot_input : 1; // 如果先导输入故障保护处于活动状态，则为true，处理在操作过程中断开操纵杆之类的事情
uint8_t gcs : 1; // 地面站故障保护的状态标志
uint8_t ekf : 1; // 如果发生ekf故障安全，则为true
uint8_t terrain : 1; // 如果发生了丢失的地形数据故障保险，则为true
uint8_t leak : 1; // 如果最近检测到泄漏，则为true
uint8_t internal_pressure : 1; // 如果内部压力超过阈值，则为true
uint8_t internal_temperature : 1; // 如果温度超过阈值，则为true
uint8_t crash : 1; // 如果坠毁则为真
uint8_t sensor_health : 1; // 如果至少一个传感器已触发故障保护（当前仅用于启用深度的深度模式），则为true
} failsafe;
失效保护部分，failsafe结构体内部主要分为两部分，包括最新一次故障时间的记录以及故障标志位更新。
// sensor health for logging
struct {
uint8_t baro : 1; // true if any single baro is healthy
uint8_t depth : 1; // true if depth sensor is healthy
uint8_t compass : 1; // true if compass is healthy
} sensor_health;
传感器健康状态监测
AP_Motors6DOF motors; //电机库定义
剩下的一些都是零散的传感器、飞行模式、控制模式等的参数变量的声明，还有一大段的函数声明，讲起来太过于零散了，所以大家还是自己去看一下源码吧。
4.2 .cpp
Sub.cpp文件中实现了Sub类的构造，代码如下
Sub::Sub()
: logger(g.log_bitmask),
control_mode(MANUAL), // 控制模式为手动
motors(MAIN_LOOP_RATE), // 电机初始化配置为400Hz运行频率
scaleLongDown(1), // GPS比例尺设置为1
auto_mode(Auto_WP), // 自动飞行模式：指向下一个航路点
guided_mode(Guided_WP), // 引导模式：指向下一个航路点
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP), // 自动飞行模式的偏航模式:指向下一个航路点（不接受飞行员输入）
inertial_nav(ahrs), // 惯性导航模式配置，默认使用NavEKF
ahrs_view(ahrs, ROTATION_NONE), // 用于创建车辆姿态的第二视图
attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS), // 姿态控制配置
pos_control(ahrs_view, inertial_nav, motors, attitude_control), // 位置控制配置
wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), // 航点导航模式配置
loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), // 游荡模式配置
circle_nav(inertial_nav, ahrs_view, pos_control), // 绕圆运动模式配置
param_loader(var_info) //参数表配置
{
// init sensor error logging flags
sensor_health.baro = true; // 设置压强传感器状态为正常
sensor_health.compass = true; // 设置电子罗盘状态为正常
#if CONFIG_HAL_BOARD != HAL_BOARD_SITL //如果不处于SITL仿真状态，那么...
failsafe.pilot_input = true; //如果飞行员输入故障保护处于活动状态，则为true，可处理操作杆在操作过程中断开连接
#endif
}
在这份文件内主要是通过Sub的构造函数实现了其他组件对象初始化，以上的所有参数均在Sub.h中有所声明。我这边简单说一下，以 control_mode(MANUAL) 为例，它在Sub.h中的Sub类中声明为一个control_mode_t 对象。
control_mode_t control_mode;
而这个control_mode_t 则是一个枚举类型
// Auto Pilot Modes enumeration
enum control_mode_t : uint8_t {
STABILIZE = 0, // manual angle with manual depth/throttle
ACRO = 1, // manual body-frame angular rate with manual depth/throttle
ALT_HOLD = 2, // manual angle with automatic depth/throttle
AUTO = 3, // fully automatic waypoint control using mission commands
GUIDED = 4, // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
CIRCLE = 7, // automatic circular flight with automatic throttle
SURFACE = 9, // automatically return to surface, pilot maintains horizontal control
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
MANUAL = 19, // Pass-through input with no stabilization
MOTOR_DETECT = 20 // Automatically detect motors orientation
};
因此 control_mode(MANUAL) 就是control_mode初始化配置为MANUAL，也就是初始化为19的意思。其他的如类对象的初始化构造相同。
关于各个飞行模式，具体可以参考官方手册中对Copter飞行模式的说明：Full list of flight modes
需要注意的是Sub类实现了对AP_Vehicle类的继承，但是其并为对setup()和loop()进行修改或者重写，而相对应的，在其内部另外声明了私有部分函数fast_loop()和init_ardupilot()，并在Ardusub.cpp和system.cpp中对两个函数进行了具体的实现。
private:
...
...
void fast_loop() override;
...
...
void init_ardupilot() override;
五、总结
总的来说，Sub类中实现的就是关于水下航行器相关功能以及传感器的最初定义。以上内容部分可能过于浅显，也可能会有出错之处，希望大家多多包涵（如有错误请务必留言指出）。后续应该还会再修改整理一下的（第一次更新：2020/10/06）， 现在就先将就看看吧。
|
