Target_Angle[0]=-Pitch_Control;//自稳时，期望俯仰角

Target_Angle[1]=-Roll_Control;//自稳时，期望横滚角

姿态自稳时候：（油门直接来源于遥控器油门给定）

        Total_Controller.Pitch_Angle_Control.Expect=Target_Angle[0];

        Total_Controller.Roll_Angle_Control.Expect=Target_Angle[1];

定高自稳时候：（水平姿态期望角来源于遥控器）

光流定位

【当前值】

OpticalFlow_Pos_Ctrl_Expect.x=OpticalFlow_SINS.Position[_PITCH];

OpticalFlow_Pos_Ctrl_Expect.y=OpticalFlow_SINS.Position[_ROLL];

accel_to_lean_angles(accel_target,&angle_target);//期望运动加速度转期望姿态倾角

Total_Controller.Pitch_Angle_Control.Expect=angle_target.y;

 Total_Controller.Roll_Angle_Control.Expect=angle_target.x;

void OpticalFlow_Pos_Control(void)

{

  static uint16_t OpticalFlow_Pos_Ctrl_Cnt=0;

  OpticalFlow_Pos_Ctrl_Cnt++;

  if(OpticalFlow_Pos_Ctrl_Cnt>=10)//50ms控制一次速度，避免输入频率过大，系统响应不过来

  {

    //计算位置偏差 

    OpticalFlow_Pos_Ctrl_Err.x=OpticalFlow_Pos_Ctrl_Expect.x-OpticalFlow_SINS.Position[_PITCH];

    OpticalFlow_Pos_Ctrl_Err.y=OpticalFlow_Pos_Ctrl_Expect.y-OpticalFlow_SINS.Position[_ROLL];

    //计算位置控制输出

    OpticalFlow_Pos_Ctrl_Output.x=Total_Controller.Optical_Position_Control.Kp*OpticalFlow_Pos_Ctrl_Err.x;

    OpticalFlow_Pos_Ctrl_Output.y=Total_Controller.Optical_Position_Control.Kp*OpticalFlow_Pos_Ctrl_Err.y;

    OpticalFlow_Pos_Ctrl_Cnt=0;

  }

}

###光流卡点

【外环】OpticalFlow_Pos_Ctrl

 OpticalFlow_Pos_Ctrl_Expect .x   = OpticalFlow_SINS.Position[_PITCH]    或   0 （打杆回中）

        ------> OpticalFlow_Pos_Ctrl_Output.x 

【内环】期望为OpticalFlow_Pos_Ctrl_Output.x   当前值为OpticalFlow_SINS.Speed[_pitch]

       ------>OpticalFlow_Ctrol_Output.x------>accel_target.x

【加速度换角度】

    accel_forward=_accel_target.y;//cm/s^2        accel_right  =_accel_target.x;//cm/s^2

    ------>target_angle->y=constrain_float(fast_atan(accel_forward*Cos_Roll/(GRAVITY_MSS*100))*RAD2DEG,-lean_angle_max,lean_angle_max);//pitch

   Total_Controller.Pitch_Angle_Control.Expect  !!!![得到了总控制量]