ArduCopter-3.2.1 源码解读
本文针对多轴飞行器最简单常见的stabilize_run模式（即自稳模式）的代码进行解读。
首先，在进行代码解读之前，首先我们要先想好，一个飞行器要工作在stabilize_run模式，需要什么输入，最后输出了什么东西。很容易地，我们知道在stabilize_run模式下，飞行器要周期性地检测陀螺仪信号来更新计算目前的角度，检测加速度计和电子罗盘来修正分别修正pitch、roll和Yaw；同时要检测遥控器的输入，转化为相应的油门值、pitch目标值、roll目标值、Yaw转动速度目标值。然后把上面的输入进行PID运算，最后输出各个电机的PWM值。
因此，我们需要从ArduCopter-3.2.1源码中到下面的程序：
a.程序是如何周期性运行的；
b.程序在哪里更新各个传感器的值（加速度计、陀螺仪、电子罗盘）；
c.飞行器姿态解算；
d.遥控器的输入是在哪些程序检测，并转换成pitch、roll、yaw目标值的；
e.PID的计算；
我心中的黄河作文朝鲜人口有多少f.电机PWM值输出；
下面，我们根据上面的思路到相应的程序并进行解读。
程序的周期性运行
首先，我们先了解程序是如何周期性运行的。在ArduPilot的“Scheduling Code to Run Intermittently (Code Overview)”做出了介绍 （网址：dev.ardupilot/wiki/apmcopter-code-overview/code-overview-scheduling-your-new-code-to-run-intermittently/）。要让程序周期性运行，有两种办法，一种是在 AP_Scheduler::Task scheduler_tasks[] PROGMEM列表里面添加函数，并定义多少时间运行一次以及设置超时，另一种是在fast_loop()函数里面新增代码。
scheduler_tasks[]可以在ArduCopter.cpp里到：
3朵玫瑰代表什么从里面我们可以看到，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::detect);的操作。因此_backends指向的是AP_InertialSensor_PX4，最后，我们在AP_InertialSensor_PX4.cpp里面到了bool AP_InertialSensor_PX4::update(void)的定义：（使用_publish_accel等函数更新传感器的值）
绕迷宫绕了大半圈，终于把加速度计陀螺仪的检测程序稍微读懂了。下面回到函数AP_AH
RS_DCM::update(void)，继续往下看程序是怎样更新姿态的。_ins.update()获取完传感器的值后，matrix_update(delta_t)就是使用刚刚测量出来的陀螺仪值、以及上个周期对陀螺仪的补偿值进行角度更新的函数，normalize()用来做余弦矩阵的归一化，drift_correction()则是使用电子罗盘、加速度计、和GPS来计算角度误差，并更新_omega（_omega会在下一个周期的matrix_update(delta_t)中被使用），其详细的算法介绍请查看，本文不再详述：wenku.baidu/link?url=ikm5-xBTm_-0wxQj2EMp7qZwtPeAwWFsQbZrwGBDrLH_c3g69ljtSGNFUd3JSR5AlRcTKzf_yQElVkXIIdduQT6HOrjT4OAQl50BpRZcQZK。上面工作完成后，在把方向余弦矩阵转换为欧拉角（函数euler_angles();），生成我们想要的pitch、roll、yaw。至此，姿态换算完毕。
计算电机输出
看完姿态换算，我们回到fast_loop()函数。read_AHRS()接着的是函数attitude_control.rate_controller_run()。该函数就是进入角速度PID运算的入口。打开函数AC_AttitudeControl::rate_controller_run()，我们进入下一轮不断嵌套来嵌套去的迷宫。
浙江高考分数线2018以_motors.set_roll(rate_bf_to_motor_roll(_rate_bf_target.x))为例，rate_bf_to_motor_roll()是使用当前的__gyro().x与_rate_bf_target.x进行PID运算，最终把运算结果赋值给_roll_control_input。 而_rate_bf_target.x是怎么来的呢，它是在void Copter::stabilize_run()里面通过下面的语句而获得的（简单的说，就是对目标角度和当前做PID运算的出的值，即双环结果的外环输出）：
版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。
发表评论