APM源码分析之 主流程

程序主流程   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     调度执行其他函数