APM源码分析之 姿态控制
姿态数据:
AP_AHRS_DCM ahrs(ins, barometer, gps) 读取处理传感器后的姿态数据
AP_InertialSensor_MPU6000 ins惯性传感器陀螺仪
定时调用_poll_dataà _read_data_transaction 函数，通过spi读取加速度
通过 update 函数 填充 AP_InertialSensor::_accel和AP_InertialSensor::_gyro 变量
罗盘数据（指向(VFR_HUD中的heading((ahrs.yaw_sensor / 100) % 360))）：
AP_Compass_HMC5843::read()->_field（修正值COMPASS_OFS 在此叠加）
Compass::calculate_heading 计算方向
->AP_AHRS_DCM::update::drift_correction(delta_t); yaw_error_compass()
-> _body_dcm_matrix= _dcm_matrix; _body_dcm_matrix.rotateXYinv(_trim)
-> _body_dcm_matrix.to_euler(&roll,&pitch, &yaw)
-> yaw_sensor = degrees(yaw) * 100对象：
AC_AttitudeControlattitude_control 姿态控制
AP_MotorsQuadmotors(g.rc_1, g.rc_2, g.rc_3, g.rc_4) 4轴飞行器控制输入
rc1:roll翻滚 rc2:pitch 俯仰 rc3: throttle 油门 rc4: yaw偏航
飞行控制(以定高模式为例):
代码执行顺序
fast_loop():
rate_controller_run 设置控制量到输出变量
AP_Motors::set_roll(rate_bf_to_motor_roll(_rate_bf_target.x))
rate_bf_to_motor_roll算法pid，计算控制量
AP_Motors ::set_pitch(rate_bf_to_motor_pitch(_rate_bf_target.y))
rate_bf_to_motor_pitch算法pid，计算控制量
AP_Motors ::set_yaw(rate_bf_to_motor_yaw(_rate_bf_target.z))
rate_bf_to_motor_yaw 算法pid，计算控制量
set_servos_4 把输出变量，输出到io (pwm 电机执行控制)
update_flight_mode
stabilize_run 稳定模式
update_simple_mode 发现有新的控制指令，执行一次简单模式,坐标转换(有头模式坐标)
get_pilot_desired_lean_angles根据输入量(指令 翻滚和俯仰)计算输出量（控制 翻滚和俯仰）
输出roll_out 和 pitch_out (value =control_in* ANGLE_MAX /4500 ）
get_pilot_desired_yaw_rate根据输入量(指令 偏航)计算输出量（控制偏航）
get_pilot_desired_throttle返回油门控制量
roll_angle_ef= roll_out pitch_angle_ef = pitch_out
angle_ef_roll_pitch_rate_ef_yaw_smooth
update_ef_yaw_angle_and_error 计算偏航量
_angle_ef_error.z= wrap_180_cd(_angle_ef_target.z -_ahrs.yaw_sensor)
_angle_ef_target.z= angle_ef_error.z + _ahrs.yaw_sensor
_angle_ef_target.z+= yaw_rate_ef * _dt
frame_conversion_ef_to_bf 坐标转换，没明白怎么回事，需要细看
update_rate_bf_targets 又他妈的坐标转换
set_throttle_out油门控制输出
althold_run 定高模式
前面和stabilize模式一样
target_climb_rate= get_pilot_desired_climb_rate 根据油门量获取期望的爬升率
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth同稳定模式
pos_control.set_alt_target_from_climb_rate(target_climb_rate,G_Dt)
_pos_target.z += target_climb_rate * G_Dt 设置当前定高高度
pos_control.update_z_controller()具体见油门部分文档数据流图:
以roll 为例，分析rate_bf_to_motor_roll （rate_target_cds）内部数据，已经pid 调节的过程
关于参数rate_target_cds，它其实是_rate_bf_target.x，也即是_angle_bf_error*_p_angle_roll，本来是目标角度与实际角度的差值，在这里变成了目标角速度，也就是说这里的pid 调节实际上是以角速度为目标的。
current_rate = (_ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100) 获取当前横滚的角速度，基本上是陀螺仪的原始数据了
dd 数据实例1,：解锁后起飞后(不实际飞起来)，不动遥控的roll，保持_angle_ef_target.x 为0，人为将机体，倾斜，此时pid控制输出应该是以机体保持水平为目标的
数据实例2：将PID参数设置为默认值，机体飞行将摇摇晃晃，不动遥控的roll，保持_angle_ef_target.x 为0
飞行姿态控制器结构（AC_AttitudeControl）
作为飞控的控制核心部分，唯一的目的是使飞机稳定的保持目标姿态，吃透了这部分代码，基本上就掌握了飞控的最核心部分，虽然代码很少，但确实很重要，所有的飞行模式都是以它为基础的
