APM_ArduCopter源码解析学习（三）——无人机类型
一、前言
本文就主要来讲一讲APM里面的无人机类型好了，当然是以Copter作为主要对象，不过前面的继承关系和基类基本都是一致的，也可以作为参考。
Copter类定义在Copter.h 文件内部，其向上继承自 AP_Vechile 类，这是所有无人机类型（Copter、Rover…）的父类。同样，AP_Vechile向上也有相应的继承关系，具体来说，用下面这种方式表达：
AP_HAL::HAL::Callbacks
|---- AP_Vechile
|---- Copter
因此，为了能对Copter类的无人机类型进行深入了解，我们还是得回到最源头讲起。
二、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
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
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 {
FunCallbacks(void (*setup_fun)(void), void (*loop_fun)(void));
void setup() override { _setup(); }
void loop() override { _loop(); }
private:
void (*_setup)(void);
void (*_loop)(void);
};
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;
}
/* Do not allow copies */
AP_Vehicle(const AP_Vehicle &other) = delete;
AP_Vehicle &operator=(const AP_Vehicle&) = delete;
static AP_Vehicle *get_singleton();
// setup() is called once during vehicle startup to initialise the
// vehicle object and the objects it contains. The
// AP_HAL_MAIN_CALLBACKS pragma creates a main(...) function
// referencing an object containing setup() and loop() functions.
// A vehicle is not expected to override setup(), but
// subclass-specific initialisation can be done in init_ardupilot
// which is called from setup().
void setup(void) override final;
// HAL::Callbacks implementation.
void loop() override final;
bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0;
uint8_t virtual get_mode() const = 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;
};
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;
/*
set the "likely flying" flag. This is not guaranteed to be
accurate, but is the vehicle codes best guess as to the whether
the vehicle is currently flying
*/
void set_likely_flying(bool b) {
if (b && !likely_flying) {
_last_flying_ms = AP_HAL::millis();
}
likely_flying = b;
}
/*
get the likely flying status. Returns true if the vehicle code
thinks we are flying at the moment. Not guaranteed to be
accurate
*/
bool get_likely_flying(void) const {
return likely_flying;
}
/*
return time in milliseconds since likely_flying was set
true. Returns zero if likely_flying is currently false
*/
uint32_t get_time_flying_ms(void) const {
if (!likely_flying) {
return 0;
}
return AP_HAL::millis() - _last_flying_ms;
}
// returns true if the vehicle has crashed
virtual bool is_crashed() const;
/*
methods to control vehicle for use by scripting
*/
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; }
// get target location (for use by scripting)
virtual bool get_target_location(Location& target_loc) { return false; }
// set steering and throttle (-1 to +1) (for use by scripting with Rover)
virtual bool set_steering_and_throttle(float steering, float throttle) { return false; }
// control outputs enumeration
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
};
// get control output (for use in scripting)
// returns true on success and control_value is set to a value in the range -1 to +1
virtual bool get_control_output(AP_Vehicle::ControlOutput control_output, float &control_value) { return false; }
// write out harmonic notch log messages
void write_notch_log_messages() const;
// update the harmonic notch
virtual void update_dynamic_notch() {};
// zeroing the RC outputs can prevent unwanted motor movement:
virtual bool should_zero_rc_outputs_on_reboot() const { return false; }
// reboot the vehicle in an orderly manner, doing various cleanups
// and flashing LEDs as appropriate
void reboot(bool hold_in_bootloader);
protected:
virtual void init_ardupilot() = 0;
virtual void load_parameters() = 0;
virtual void set_control_channels() {}
// board specific config
AP_BoardConfig BoardConfig;
#if HAL_MAX_CAN_PROTOCOL_DRIVERS
// board specific config for CAN bus
AP_CANManager can_mgr;
#endif
// main loop scheduler
AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&AP_Vehicle::fast_loop, void)};
virtual void fast_loop();
// IMU variables
// Integration time; time last loop took to run
float G_Dt;
// sensor drivers
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;
// notification object for LEDs, buzzers etc (parameter set to
// false disables external leds)
AP_Notify notify;
// Inertial Navigation EKF
#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() callback that processing MAVLink packets
static void scheduler_delay_callback();
// if there's been a watchdog reset, notify the world via a
// statustext:
void send_watchdog_reset_statustext();
bool likely_flying; // true if vehicle is probably flying
uint32_t _last_flying_ms; // time when likely_flying last went true
static AP_Vehicle *_singleton;
};
需要注意的是在公有部分内部声明的两个函数setup()和loop()。之前查了很多博客，包括截至写博客期间的官方手册，讲述的都是以前的版本。以前版本的APM源码在对应的具体车辆类型中（如Copter等）是在主文件中（如Copter.cpp）中通过setup()完成初始化，loop()完成主循环。现在的话主循环完成在fast_loop()里面。
此外在protected部分，需要注意的是init_ardupilot()和fast_loop()两个方法。
virtual void init_ardupilot() = 0;
...
virtual void fast_loop();
- 1
- 2
- 3
3.2 .cpp
在对应的AP_Vechile.cpp文件中实现了setup()，loop()和fast_loop()函数。
/*
setup is called when the sketch starts
*/
void AP_Vehicle::setup()
{
// load the default values of variables listed in var_info[]
AP_Param::setup_sketch_defaults();
// initialise 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());
load_parameters();
// initialise the main loop scheduler
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);
// time per loop - this gets updated in the main loop() based on
// actual loop rate
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();
// initialise serial manager as early as sensible to get
// diagnostic output during boot process. We have to initialise
// the GCS singleton first as it sets the global mavlink system ID
// which may get used very early on.
gcs().init();
// initialise serial ports
serial_manager.init();
gcs().setup_console();
// Register scheduler_delay_cb, which will run anytime you have
// more than 5ms remaining in your call to hal.scheduler->delay
hal.scheduler->register_delay_callback(scheduler_delay_callback, 5);
#if HAL_MSP_ENABLED
// call MSP init before init_ardupilot to allow for MSP sensors
msp.init();
#endif
// init_ardupilot is where the vehicle does most of its initialisation.
init_ardupilot();
gcs().send_text(MAV_SEVERITY_INFO, "ArduPilot Ready");
// gyro FFT needs to be initialized really late
#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
// init library used for visual position estimation
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();
}
- 1
- 2
- 3
- 4
- 5
/*
所有无人机的快速循环回调。 这将在任何特定于无人机的快速循环结束时调用。
*/
void AP_Vehicle::fast_loop()
{
#if HAL_GYROFFT_ENABLED
gyro_fft.sample_gyros();
#endif
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
总结：
- 在setup()里面完成了如串口和参数表等部分设备的初始化内容，但是具体的无人机设备初始化需要在init_ardupilot()中完成。
- loop()函数较短，内部实现的是对任务循环并且获取任务单词循环时的时间（以s为单位）。
- fast_loop()见注释。
四、class Copter
4.1 .h
内容实在是有点多，为了排版和阅读方便，这边就不全部放上来了，挑一些重点说一下。
public:
friend class GCS_MAVLINK_Copter;
friend class GCS_Copter;
friend class AP_Rally_Copter;
friend class Parameters;
friend class ParametersG2;
friend class AP_Avoidance_Copter;
#if ADVANCED_FAILSAFE == ENABLED
friend class AP_AdvancedFailsafe_Copter;
#endif
friend class AP_Arming_Copter;
friend class ToyMode;
friend class RC_Channel_Copter;
friend class RC_Channels_Copter;
friend class AutoTune;
friend class Mode;
friend class ModeAcro;
friend class ModeAcro_Heli;
friend class ModeAltHold;
friend class ModeAuto;
friend class ModeAutoTune;
friend class ModeAvoidADSB;
friend class ModeBrake;
friend class ModeCircle;
friend class ModeDrift;
friend class ModeFlip;
friend class ModeFlowHold;
friend class ModeFollow;
friend class ModeGuided;
friend class ModeLand;
friend class ModeLoiter;
friend class ModePosHold;
friend class ModeRTL;
friend class ModeSmartRTL;
friend class ModeSport;
friend class ModeStabilize;
friend class ModeStabilize_Heli;
friend class ModeSystemId;
friend class ModeThrow;
friend class ModeZigZag;
friend class ModeAutorotate;
Copter(void);
首先是公有部分，内部声明了多个个友元类，这些类中的成员函数能够访问Copter类中的私有成员。构造函数Copter()在Copter.cpp文件中实现。
// primary input control channels
RC_Channel *channel_roll;
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
这部分上一篇博客已经提过了，但是这边还是想说一下，此处表示的是Copter的主要输入控制通道。
4.2 .cpp
Copter.cpp文件中实现了Copter类的构造，代码如下
Mode::Number control_mode;
定义control_mode为STABILIZE。而Mode::Number是一个枚举类，具体在ArduCopter路径下的mode.h中进行定义
/*
constructor for main Copter class
*/
Copter::Copter(void)
: logger(g.log_bitmask),
flight_modes(&g.flight_mode1),
control_mode(Mode::Number::STABILIZE),
simple_cos_yaw(1.0f),
super_simple_cos_yaw(1.0),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
rc_throttle_control_in_filter(1.0f),
inertial_nav(ahrs),
param_loader(var_info),
flightmode(&mode_stabilize)
{
// init sensor error logging flags
sensor_health.baro = true;
sensor_health.compass = true;
}
在这份文件内主要是通过Copter的构造函数实现了其他组件对象初始化，以上的所有参数均在Copter.h中有所声明。
// Auto Pilot Modes enumeration
enum class Number : uint8_t {
STABILIZE = 0, // manual airframe angle with manual throttle
ACRO = 1, // manual body-frame angular rate with manual throttle
ALT_HOLD = 2, // manual airframe angle with automatic 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
LOITER = 5, // automatic horizontal acceleration with automatic throttle
RTL = 6, // automatic return to launching point
CIRCLE = 7, // automatic circular flight with automatic throttle
LAND = 9, // automatic landing with horizontal position control
DRIFT = 11, // semi-automous position, yaw and throttle control
SPORT = 13, // manual earth-frame angular rate control with manual throttle
FLIP = 14, // automatically flip the vehicle on the roll axis
AUTOTUNE = 15, // automatically tune the vehicle's roll and pitch gains
POSHOLD = 16, // automatic position hold with manual override, with automatic throttle
BRAKE = 17, // full-brake using inertial/GPS system, no pilot input
THROW = 18, // throw to launch mode using inertial/GPS system, no pilot input
AVOID_ADSB = 19, // automatic avoidance of obstacles in the macro scale - e.g. full-sized aircraft
GUIDED_NOGPS = 20, // guided mode but only accepts attitude and altitude
SMART_RTL = 21, // SMART_RTL returns to home by retracing its steps
FLOWHOLD = 22, // FLOWHOLD holds position with optical flow without rangefinder
FOLLOW = 23, // follow attempts to follow another vehicle or ground station
ZIGZAG = 24, // ZIGZAG mode is able to fly in a zigzag manner with predefined point A and point B
SYSTEMID = 25, // System ID mode produces automated system identification signals in the controllers
AUTOROTATE = 26, // Autonomous autorotation
};
关于各个飞行模式，具体可以参考官方手册中对Copter飞行模式的说明：Full list of flight modes
需要注意的是Copter类实现了对AP_Vehicle类的继承，但是其并为对setup()和loop()进行修改或者重写，而相对应的，在其内部另外声明了私有部分函数fast_loop()和init_ardupilot()，并在Copter.cpp和system.cpp中对两个函数进行了具体的实现。以下是Copter.h中对于这两个函数的引用部分。
void fast_loop() override;
// system.cpp
void init_ardupilot() override;
五、总结
总的来说，Copter类中实现的就是关于无人机相关功能以及传感器的最初定义。以上内容部分可能过于浅显，也可能会有出错之处，希望大家多多包涵（如有错误请务必留言指出）。后续应该还会再修改整理一下的（第一次更新：2020/12/02）， 现在就先将就看看吧。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
