Ardupilot入口分析

▉<=1=>▉---------------------------------------------------------------->>>>>>
入口函数分析
▉<=1=>▉---------------------------------------------------------------->>>>>>
 

ArduCopter.cpp 文件的最后一行为整个代码入口main,展开之后
int main(int argc, char *const argv[]);
int main(int argc, char *const argv[]) 
{
hal.run(argc, argv, &copter);
return 0;
}
    分两部分分析：<1>.copter,<2>. hal.run(...)

<1>.这里的 copter 对象实例化 在Copter.cpp 文件中:
Copter copter;
Copter 从 Callbacks抽象类公有派生而来：
struct Callbacks {// Callbacks 成员
virtual void setup() = 0; // callbacks 纯虚函数成员方法
virtual void loop() = 0;  // claabacks 纯虚函数成员方法
}; 

class Copter : public AP_HAL::HAL::Callbacks {
public:
......
// HAL::Callbacks implementation.
void setup() override; // 该方法具体实现在ArduCopter.cpp
void loop() override;  // 该方法具体实现在ArduCopter.cpp
......
};
其中：
A:
void Copter::setup() 
{
cliSerial = hal.console;


// Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();


// setup storage layout for copter
StorageManager::set_layout_copter();


init_ardupilot(); // 包含整个ardupilot系统的初始化


// initialise the main loop scheduler
// 初始化 Auducopter.cpp 开头的 scheduler_tasks 所有任务数组
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));


// setup initial performance counters
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}
B:
void Copter::loop()
{
// wait for an INS sample
ins.wait_for_sample();


uint32_t timer = micros();


// check loop time
perf_info_check_loop_time(timer - fast_loopTimer);


// used by PI Loops
G_Dt = (float)(timer - fast_loopTimer) / 1000000.0f;
fast_loopTimer = timer;


// for mainloop failure monitoring
mainLoop_count++;


// Execute the fast loop
// ---------------------
fast_loop();


// tell the scheduler one tick has passed
scheduler.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
uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros();
scheduler.run(time_available > MAIN_LOOP_MICROS ? 0u : time_available);
}

<2>.这里的 hal 对象实例化在 Copter.cpp 文件中:
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
A. 其中 AP_HAL 为 namespace, 
namespace AP_HAL {
/* Toplevel pure virtual class Hal.顶层纯虚类HAL*/ 
class HAL;


/* Toplevel class names for drivers: 顶层驱动相关类*/
class UARTDriver;
class I2CDevice;
class I2CDeviceManager;
class Device;


class SPIDevice;
class SPIDeviceDriver;
class SPIDeviceManager;


class AnalogSource;
class AnalogIn;
class Storage;
class DigitalSource;
class GPIO;
class RCInput;
class RCOutput;
class Scheduler;
class Semaphore;
class OpticalFlow;


class Util;


/* Utility Classes 通用实用的类*/
class Print;
class Stream;
class BetterStream;


/* Typdefs for function pointers (Procedure, Member Procedure)
  函数指针类型定义
  For member functions(成员函数) we use the FastDelegate delegates(代表) class
  which allows us to encapculate(封装) a member function as a type
*/
typedef void(*Proc)(void);
FUNCTOR_TYPEDEF(MemberProc, void);


/**
* Global names for all of the existing(已有的) SPI devices on all platforms.
*/


enum SPIDeviceType {
// Devices using AP_HAL::SPIDevice abstraction(抽象)
SPIDevice_Type = -1,
};


// Must be implemented(执行)by the concrete(具体的) HALs.
const HAL& get_HAL();
}


B. HAL 类头文件 hal.h
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::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)
:
uartA(_uartA),
uartB(_uartB),
uartC(_uartC),
uartD(_uartD),
uartE(_uartE),
uartF(_uartF),
i2c_mgr(_i2c_mgr),
spi(_spi),
analogin(_analogin),
storage(_storage),
console(_console),
gpio(_gpio),
rcin(_rcin),
rcout(_rcout),
scheduler(_scheduler),
util(_util),
opticalflow(_opticalflow)
{
AP_HAL::init(); //AP_HAL 构造函数
}


struct Callbacks {// Callbacks 成员
virtual void setup() = 0; // callbacks 纯虚函数成员方法
virtual void loop() = 0;  // callbacks 纯虚函数成员方法
};


struct FunCallbacks : public Callbacks { // FunCallbacks 成员派生自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::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;
};


C. 而get_HAL一共6个函数, 尚不知系统如何选择，这里暂时选择 HAL_PX4_Class.cpp 文件的：
const AP_HAL::HAL& AP_HAL::get_HAL() {
static const HAL_PX4 hal_px4;
return hal_px4;
}
这里，实例化了一个HAL_PX4 对象 hal_px4, 并返回其引用；
而 HAL_PX4 由 AP_HAL 名字空间中的 HAL 抽象类派生而来
class HAL_PX4 : public AP_HAL::HAL {
public:
   HAL_PX4();
// 被派生类重写override的虚函数run
   void run(int argc, char* const argv[], Callbacks* callbacks) const override; 
};
这里返回了一个派生类对象给基类，应该是：通过指向派生类对象的基类指针调用派生类的虚函数而多态。

综上：hal.run() 实际执行 hal_px4.run(argc, argv, &copter);
下面是最终调用的run函数：
HAL_PX4_Class.cpp (ardupilot\libraries\ap_hal_px4)
这里，copter 派生类指针被隐式转化为基类指针Callbacks*，即 多态 行为
void HAL_PX4::run(int argc, char * const argv[], Callbacks* callbacks) const
{
int i;
const char *deviceA = UARTA_DEFAULT_DEVICE;
const char *deviceC = UARTC_DEFAULT_DEVICE;
const char *deviceD = UARTD_DEFAULT_DEVICE;
const char *deviceE = UARTE_DEFAULT_DEVICE;


if (argc < 1) {
printf("%s: missing command (try '%s start')", 
  SKETCHNAME, SKETCHNAME);
usage();
exit(1);
}


assert(callbacks);
// 定义在 HAL_PX4_Class.cpp (ardupilot\libraries\ap_hal_px4)
   // static AP_HAL::HAL::Callbacks* g_callbacks;
g_callbacks = callbacks;  // &copter 被其基类指针 Callbacks* 所指向。


for (i=0; i<argc; i++) {
if (strcmp(argv[i], "start") == 0) {
if (thread_running) {
printf("%s already running\n", SKETCHNAME);
/* this is not an error */
exit(0);
}


uartADriver.set_device_path(deviceA);
uartCDriver.set_device_path(deviceC);
uartDDriver.set_device_path(deviceD);
uartEDriver.set_device_path(deviceE);
printf("Starting %s uartA=%s uartC=%s uartD=%s uartE=%s\n", 
  SKETCHNAME, deviceA, deviceC, deviceD, deviceE);


_px4_thread_should_exit = false;
//创建守护进程
daemon_task = px4_task_spawn_cmd(SKETCHNAME,
SCHED_FIFO,
APM_MAIN_PRIORITY,
APM_MAIN_THREAD_STACK_SIZE,
main_loop, // 主循环，后面分析------------------------->
NULL);
exit(0);
}


if (strcmp(argv[i], "stop") == 0) {
_px4_thread_should_exit = true;
exit(0);
}
 
if (strcmp(argv[i], "status") == 0) {
if (_px4_thread_should_exit && thread_running) {
printf("\t%s is exiting\n", SKETCHNAME);
} else if (thread_running) {
printf("\t%s is running\n", SKETCHNAME);
} else {
printf("\t%s is not started\n", SKETCHNAME);
}
exit(0);
}


if (strcmp(argv[i], "-d") == 0) {
// set terminal device
if (argc > i + 1) {
deviceA = strdup(argv[i+1]);
} else {
printf("missing parameter to -d DEVICE\n");
usage();
exit(1);
}
}


if (strcmp(argv[i], "-d2") == 0) {
// set uartC terminal device
if (argc > i + 1) {
deviceC = strdup(argv[i+1]);
} else {
printf("missing parameter to -d2 DEVICE\n");
usage();
exit(1);
}
}


if (strcmp(argv[i], "-d3") == 0) {
// set uartD terminal device
if (argc > i + 1) {
deviceD = strdup(argv[i+1]);
} else {
printf("missing parameter to -d3 DEVICE\n");
usage();
exit(1);
}
}


if (strcmp(argv[i], "-d4") == 0) {
// set uartE 2nd GPS device
if (argc > i + 1) {
deviceE = strdup(argv[i+1]);
} else {
printf("missing parameter to -d4 DEVICE\n");
usage();
exit(1);
}
}
}
 
usage();
exit(1);
}
接上，分析main_loop
static int main_loop(int argc, char **argv)
{
hal.uartA->begin(115200);
hal.uartB->begin(38400);
hal.uartC->begin(57600);
hal.uartD->begin(57600);
hal.uartE->begin(57600);
hal.scheduler->init();
hal.rcin->init();
hal.rcout->init();
hal.analogin->init();
hal.gpio->init();

//  run setup() at low priority to ensure CLI(command line interface) does not hang the
//  system, and to allow initial sensor read loops to run

hal_px4_set_priority(APM_STARTUP_PRIORITY);


schedulerInstance.hal_initialized();

// 多态调用，基类Callbacks指针调用派生类Copter的重写的虚函数setup，
// 在前面<1>.A ---> ArduCopter.cpp (ardupilot\arducopter)
g_callbacks->setup();
hal.scheduler->system_initialized();


perf_counter_t perf_loop = perf_alloc(PC_ELAPSED, "APM_loop");
perf_counter_t perf_overrun = perf_alloc(PC_COUNT, "APM_overrun");
struct hrt_call loop_overtime_call;

// 定义static bool thread_running = false;        /**< Daemon status flag */
thread_running = true;


/*
 switch to high priority for main loop
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);

/*****************************************
* 整个飞控系统的主循环while：
*****************************************/
while (!_px4_thread_should_exit) { // 定义 bool _px4_thread_should_exit = false;        /**< Daemon exit flag */
perf_begin(perf_loop);

/*
 this ensures a tight(紧凑的) loop waiting on a lower priority driver
 will eventually give up some time for the driver to run. It
 will only ever be called if a loop() call runs for more than
 0.1 second
*/
hrt_call_after(&loop_overtime_call, 100000, (hrt_callout)loop_overtime, NULL);

// 多态调用，基类Callbacks指针调用派生类Copter的重写的虚函数loop，
// 在前面<1>.B ---> ArduCopter.cpp (ardupilot\arducopter)
g_callbacks->loop();

if (px4_ran_overtime) {
/*
 we ran over 1s in loop(), and our priority was lowered
 to let a driver run. Set it back to high priority now.
*/
hal_px4_set_priority(APM_MAIN_PRIORITY);
perf_count(perf_overrun);
px4_ran_overtime = false;
}


perf_end(perf_loop);


/*
 give up 250 microseconds of time, to ensure drivers get a
 chance to run. This relies on the accurate semaphore wait
 using hrt(high resolution timer) in semaphore.cpp
*/
hal.scheduler->delay_microseconds(250);
}
thread_running = false;
return 0;
}