Ardupilot-ArduCopter-3.2.1 源码解读.docxVIP

ArduCopter-3.2.1源码解读本文针对多轴飞行器最简单常见的stabilize_run模式（即自稳模式）的代码进行解读。首先，在进行代码解读之前，首先我们要先想好，一个飞行器要工作在stabilize_run模式，需要什么输入，最后输出了什么东西。很容易地，我们知道在stabilize_run模式下，飞行器要周期性地检测陀螺仪信号来更新计算目前的角度，检测加速度计和电子罗盘来修正分别修正pitch、roll和Yaw；同时要检测遥控器的输入，转化为相应的油门值、pitch目标值、roll目标值、Yaw转动速度目标值。然后把上面的输入进行PID运算，最后输出各个电机的PWM值。因此，我们需要从ArduCopter-3.2.1源码中找到下面的程序：程序是如何周期性运行的；程序在哪里更新各个传感器的值（加速度计、陀螺仪、电子罗盘）；飞行器姿态解算；遥控器的输入是在哪些程序检测，并转换成pitch、roll、yaw目标值的；PID的计算；电机PWM值输出；下面，我们根据上面的思路找到相应的程序并进行解读。程序的周期性运行首先，我们先了解程序是如何周期性运行的。在ArduPilot官网的“Scheduling Code to Run Intermittently (Code Overview)”做出了介绍（网址：/wiki/apmcopter-code-overview/code-overview-scheduling-your-new-code-to-run-intermittently/）。要让程序周期性运行，有两种办法，一种是在AP_Scheduler::Taskscheduler_tasks[] PROGMEM列表里面添加函数，并定义多少时间运行一次以及设置超时，另一种是在fast_loop()函数里面新增代码。scheduler_tasks[]可以在ArduCopter.cpp里找到：从里面我们可以看到，scheduler_tasks的程序，最快执行周期是2.5ms（硬件对应pixhawk）。并且里面的程序包括rc_loop和throttle_loop，这两个程序就是从遥控器接收油门、yaw、pitch、roll控制量的程序（不过我没细看这两个程序，也没想明白为什么没把throttle当做遥控器的一部分，高那么复杂）。其中rc_loop的执行周期是4*2.5=10ms。另外一个周期性函数，就是fast_loop()了。在ArduCopter.cpp我们可以找到它，它的执行时间是2.5ms。更新传感器和更新姿态程序周期性运行的问题解决了，接下来我们找程序在哪里更新各个传感器的值，在哪里更新姿态、进行PID运算以及输出pwm。其实在程序fast_loop()里面，就找到了。read_AHRS()就是更新传感器并更新姿态的函数；attitude_control.rate_controller_run()是进行角速度PID运算的函数；motors_output()是输出电机PWM值的函数。下面分别对这些函数作一一分解并进行分析。read_AHRS()里面调用的是ahrs.update()，ahrs是类AP_AHRS_DCM的一个实例，因此我们在AP_AHRS_DCM.cpp找到了AP_AHRS_DCM::update(void)的定义：AP_AHRS_DCM::update(void)里面，使用_ins.update()来更新陀螺仪加速度计。在这个函数里面，调用_backends[i]-update();这里才是更新陀螺仪加速度计动作的函数。但是很多人在这里就无法进一步往下看了，因为AP_InertialSensor_Backend的update()是没有进一步定义的。我们去找_backends的定义：AP_InertialSensor_Backend *_backends[INS_MAX_BACKENDS];可以看到_backends[]是个指针数组，我们找到这些指针指向哪些变量，就可以找到该变量对应的类里面的update()函数了（这个不得不表达一下对ArduCopter源码的怨恨，太多的嵌套，太多的子类，找个函数像找迷宫一样。我无法想象结构这么不清晰的程序是怎么维护的）。通过有哪些信誉好的足球投注网站_backends，在AP_InertialSensor.cpp找到了void AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *(detect)(AP_InertialSensor ))函数的定义，再找到了AP_InertialSensor::_detect_backends(void)函数里进行了_add_backend(AP_InertialSensor_PX4::dete