程序主流程 AP_HAL_MAIN(main 函数定义)
hal.init(0, NULL)(执行 HAL_AVR_APM2::init)
scheduler->init 调度模块初始化
uartA->begin(115200, 128, 128) 启动串口
rcin->init 输入初始化
rcout->init 输出初始化
spi i2c c初始化
setup()
AP_Param::setup_sketch_defaults
StorageManager::set_layout_copter
init_ardupilot 初始化操作
report_version 版本输出
load_parameters 加载参数
BoardConfig.init
notify.init 初始化对外信号系统
batterybarometer gcs rc_in rc_out gps inertial_nav电池 气压计 gcs等 初始化
scheduler.init 调度模块初始化(装载调度函数)
for(;;)loop()
fast_loop()
read_AHRS读取飞行器姿态等基础数据
gcs_check_input处理来自地面站的命令
ahrs.update ahrs为姿态航向对象 AP_AHRS_NavEKF(ekf滤波) 或 AP_AHRS_DCM
_ins.update _ins为惯性传感器对象(AP_InertialSensor_MPU6000)即 陀螺仪
matrix_update根据陀螺仪数据转换(坐标变换)的矩阵数据
drift_correction 偏航修正 计算_error_yaw_sum 累加值
euler_angles 欧拉角度计算,计算得到飞行姿态数据
update_trig 计算 获取_cos_roll, cos_pitch _sin_pitch sin_pitch值
attitude_control.rate_controller_run 向电机对象设置输出目标姿态值
set_servos_4 计算各电机pwm输出
read_inertia 惯性导航对象(AP_InertialNav)数据更新
inertial_nav.update 主要是计算高度位置,速度
update_flight_mode 根据飞行模式调用控制函数
scheduler.run 调度执行其他函数