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）

作为飞控的控制核心部分，唯一的目的是使飞机稳定的保持目标姿态，吃透了这部分代码，基本上就掌握了飞控的最核心部分，虽然代码很少，但确实很重要，所有的飞行模式都是以它为基础的