#include "ti_msp_dl_config.h"
#include "systick.h"
#include "sensor.h"
#include "pid.h"
#include "user_ctrl.h"

Controller_Output Flight;


//将NED坐标系下的期望加速度转换成机体坐标系下的倾斜角度(pitch,roll)
void Desired_Accel_Transform_Angle(vector2f _accel_target, vector2f *target_angle)
{
  float accel_right, accel_forward;
  float lean_angle_max = 30.0f;
  accel_right  = -_accel_target.x; //cm/s^2
  accel_forward = _accel_target.y; //cm/s^2
  //update angle targets that will be passed to stabilize controller
  //计算期望横滚角
  target_angle->x = RAD_TO_DEG*atanf((-accel_right / (GRAVITY_MSS * 100)) * uav_ahrs.cos_rpy[_PIT]);
  //计算期望俯仰角
  target_angle->y = RAD_TO_DEG*atanf(accel_forward / (GRAVITY_MSS * 100));

  target_angle->x = constrain_float(target_angle->x, -lean_angle_max, lean_angle_max); //roll
  target_angle->y = constrain_float(target_angle->y, -lean_angle_max, lean_angle_max); //pitch
}


systime althold_dt;
vector3f nav_target_vel = {0, 0, 0};
void Flight_Alt_Hold_Control(uint8_t mode, float target_alt, float target_vel)
{
  //static uint8_t last_mode=0;
  //uint8_t alt_fixed_flag=0;
  get_systime(&althold_dt);

  //如果本函数运行周期大于控制周期10倍
  if(0.001f * althold_dt.period >= 10 * WP_Duty_Dt)
  {
    //情况1:初次或者从其它模式切入本模式
    //情况2:系统调度超时，在系统设计合理情况下，本情况不可能发生
    //alt_fixed_flag=1;
  }

  //高度控制从自动模式切到手动模式
  //if(last_mode!=mode)  alt_fixed_flag=1;
  //last_mode=mode;
  switch(mode)
  {
    case ALTHOLD_MANUAL_CTRL:
    {
      Total_Controller.Height_Position_Control.FeedBack = uav_ins.position[_UP];	//高度位置反馈
      PID_Control(&Total_Controller.Height_Position_Control, 0.005f);									//海拔高度位置控制器
      nav_target_vel.z = Total_Controller.Height_Position_Control.Control_OutPut;
    }
    break;

    case ALTHOLD_AUTO_POS_CTRL:
    {
      //1、高度位置控制器
      //高度位置环输出给定速度期望
      Total_Controller.Height_Position_Control.Expect = target_alt;	//油门处于回中后，更新高度期望
      Total_Controller.Height_Position_Control.FeedBack = uav_ins.position[_UP];	//高度位置反馈
      PID_Control(&Total_Controller.Height_Position_Control, 0.005f);									//海拔高度位置控制器

      nav_target_vel.z = Total_Controller.Height_Position_Control.Control_OutPut;
    }
    break;

    case ALTHOLD_AUTO_VEL_CTRL:
    {
      nav_target_vel.z = target_vel;
    }
    break;
  }

  nav_target_vel.z = constrain_float(nav_target_vel.z, -500, 500); //油门来源于高度速度控制器输出
}


uint8_t  OpticalFlow_Pos_Ctrl_Recode = 1;
vector2f OpticalFlow_Pos_Ctrl_Expect = {0, 0};
vector2f OpticalFlow_Pos_Ctrl_Err = {0, 0};
vector2f OpticalFlow_Pos_Ctrl_Integrate = {0, 0};
vector2f OpticalFlow_Pos_Ctrl_Output = {0, 0};

vector2f OpticalFlow_Vel_Ctrl_Expect = {0, 0};
vector2f OpticalFlow_Vel_Ctrl_Err = {0, 0};
vector2f OpticalFlow_Vel_Ctrl_Integrate = {0, 0};
vector2f OpticalFlow_Vel_Ctrl_Output = {0, 0};

vector2f accel_target = {0, 0}, angle_target = {0, 0};

void OpticalFlow_Ctrl_Reset(void)
{
  OpticalFlow_Vel_Ctrl_Integrate.x = 0.0f;
  OpticalFlow_Vel_Ctrl_Integrate.y = 0.0f;
  OpticalFlow_Pos_Ctrl_Integrate.x = 0.0f;
  OpticalFlow_Pos_Ctrl_Integrate.y = 0.0f;
  OpticalFlow_Pos_Ctrl_Expect.x = uav_ins.position[_EAST];
  OpticalFlow_Pos_Ctrl_Expect.y = uav_ins.position[_NORTH];
  OpticalFlow_Pos_Ctrl_Recode = 1; //复位之后再次执行控制时会刷新悬停点
  OpticalFlow_Vel_Ctrl_Expect.x = 0;
  OpticalFlow_Vel_Ctrl_Expect.y = 0;
}


void OpticalFlow_Pos_Control_VIO(void)
{
  static uint16_t OpticalFlow_Pos_Ctrl_Cnt = 0;
  OpticalFlow_Pos_Ctrl_Cnt++;

  if(OpticalFlow_Pos_Ctrl_Cnt >= 4) //20ms控制一次速度，避免输入频率过大，系统响应不过来
  {
    //计算位置偏差
    OpticalFlow_Pos_Ctrl_Err.x = OpticalFlow_Pos_Ctrl_Expect.x - uav_ins.position[_EAST];
    OpticalFlow_Pos_Ctrl_Err.y = OpticalFlow_Pos_Ctrl_Expect.y - uav_ins.position[_NORTH];

    OpticalFlow_Pos_Ctrl_Err.x = constrain_float(OpticalFlow_Pos_Ctrl_Err.x, -Total_Controller.Optical_Position_Control.Err_Max, Total_Controller.Optical_Position_Control.Err_Max);
    OpticalFlow_Pos_Ctrl_Err.y = constrain_float(OpticalFlow_Pos_Ctrl_Err.y, -Total_Controller.Optical_Position_Control.Err_Max, Total_Controller.Optical_Position_Control.Err_Max);

    //导航坐标系下机体Pitch、Roll方向上位置偏差
    from_vio_to_body_frame(Total_Controller.Optical_Position_Control.Kp*OpticalFlow_Pos_Ctrl_Err.x,
                           Total_Controller.Optical_Position_Control.Kp*OpticalFlow_Pos_Ctrl_Err.y,
                           &OpticalFlow_Pos_Ctrl_Output.x,
                           &OpticalFlow_Pos_Ctrl_Output.y,
                           uav_ahrs.yaw);
    OpticalFlow_Pos_Ctrl_Cnt = 0;
  }
}

systime opt_speed_dt;
void OpticalFlow_Vel_Control(vector2f target)
{
  get_systime(&opt_speed_dt);

  //如果本函数运行周期大于控制周期10倍
  if(0.001f * opt_speed_dt.period >= 20 * WP_Duty_Dt)
  {
    //情况1:初次从其它模式切入本模式
    //情况2:系统调度超时，在系统设计合理情况下，本情况不可能发生
  }

  static uint16_t OpticalFlow_Vel_Ctrl_Cnt = 0;
  OpticalFlow_Vel_Ctrl_Expect.x = target.x; //速度期望
  OpticalFlow_Vel_Ctrl_Expect.y = target.y;
  OpticalFlow_Vel_Ctrl_Cnt++;

  if(OpticalFlow_Vel_Ctrl_Cnt >= 2) //10ms控制一次速度，避免输入频率过大，系统响应不过来
  {
    OpticalFlow_Vel_Ctrl_Cnt = 0;
    OpticalFlow_Vel_Ctrl_Err.x = constrain_float(OpticalFlow_Vel_Ctrl_Expect.x - uav_ins._speed[_EAST], -Total_Controller.Optical_Speed_Control.Err_Max, Total_Controller.Optical_Speed_Control.Err_Max); //30
    OpticalFlow_Vel_Ctrl_Err.y = constrain_float(OpticalFlow_Vel_Ctrl_Expect.y - uav_ins._speed[_NORTH], -Total_Controller.Optical_Speed_Control.Err_Max, Total_Controller.Optical_Speed_Control.Err_Max);

    //if(ABS(OpticalFlow_Vel_Ctrl_Err.x)<=Total_Controller.Optical_Speed_Control.Integrate_Separation_Err)
    OpticalFlow_Vel_Ctrl_Integrate.x += Total_Controller.Optical_Speed_Control.Ki*OpticalFlow_Vel_Ctrl_Err.x;
    //if(ABS(OpticalFlow_Vel_Ctrl_Err.y)<=Total_Controller.Optical_Speed_Control.Integrate_Separation_Err)
    OpticalFlow_Vel_Ctrl_Integrate.y += Total_Controller.Optical_Speed_Control.Ki*OpticalFlow_Vel_Ctrl_Err.y;

    OpticalFlow_Vel_Ctrl_Integrate.x = constrain_float(OpticalFlow_Vel_Ctrl_Integrate.x, -Total_Controller.Optical_Speed_Control.Integrate_Max, Total_Controller.Optical_Speed_Control.Integrate_Max);
    OpticalFlow_Vel_Ctrl_Integrate.y = constrain_float(OpticalFlow_Vel_Ctrl_Integrate.y, -Total_Controller.Optical_Speed_Control.Integrate_Max, Total_Controller.Optical_Speed_Control.Integrate_Max);

    OpticalFlow_Vel_Ctrl_Output.x = OpticalFlow_Vel_Ctrl_Integrate.x + Total_Controller.Optical_Speed_Control.Kp*OpticalFlow_Vel_Ctrl_Err.x; //4.5
    OpticalFlow_Vel_Ctrl_Output.y = OpticalFlow_Vel_Ctrl_Integrate.y + Total_Controller.Optical_Speed_Control.Kp*OpticalFlow_Vel_Ctrl_Err.y;

    accel_target.y = -constrain_float(OpticalFlow_Vel_Ctrl_Output.y, -Total_Controller.Optical_Speed_Control.Control_OutPut_Limit, Total_Controller.Optical_Speed_Control.Control_OutPut_Limit); //450
    accel_target.x = -constrain_float(OpticalFlow_Vel_Ctrl_Output.x, -Total_Controller.Optical_Speed_Control.Control_OutPut_Limit, Total_Controller.Optical_Speed_Control.Control_OutPut_Limit); //期望运动加速度

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

    Flight.roll_outer_control_output = angle_target.x;
    Flight.pitch_outer_control_output = angle_target.y;
  }
}







//适用于激光雷达SLAM定位条件下，普通光流（LC307、LC302）定位条件下无效
void Horizontal_Navigation(float x, float y, float z, uint8_t nav_mode, uint8_t frame_id)
{
  if(nav_mode == RELATIVE_MODE) //相对模式
  {
    switch(frame_id)
    {
      case BODY_FRAME://机体坐标系下
      {
        float map_x = 0, map_y = 0;
        from_body_to_nav_frame(x, y, &map_x, &map_y, uav_ahrs.yaw);
        OpticalFlow_Pos_Ctrl_Expect.x = uav_ins.position[_EAST] + map_x;
        OpticalFlow_Pos_Ctrl_Expect.y = uav_ins.position[_NORTH] + map_y;
        Total_Controller.Height_Position_Control.Expect = uav_ins.position[_UP] + z;
      }
      break;

      case MAP_FRAME://导航坐标系下
      {
        OpticalFlow_Pos_Ctrl_Expect.x = uav_ins.position[_EAST] + x;
        OpticalFlow_Pos_Ctrl_Expect.y = uav_ins.position[_NORTH] + y;
        Total_Controller.Height_Position_Control.Expect = uav_ins.position[_UP] + z;
      }
      break;
    }
  }
  else if(nav_mode == GLOBAL_MODE) //全局模式
  {
    switch(frame_id)
    {
      case MAP_FRAME://导航坐标系下
      {
        OpticalFlow_Pos_Ctrl_Expect.x = x;
        OpticalFlow_Pos_Ctrl_Expect.y = y;
        Total_Controller.Height_Position_Control.Expect = z;
      }
      break;

      default://原地保持
      {
        OpticalFlow_Pos_Ctrl_Expect.x = uav_ins.position[_EAST];
        OpticalFlow_Pos_Ctrl_Expect.y = uav_ins.position[_NORTH];
        Total_Controller.Height_Position_Control.Expect = uav_ins.position[_UP];
      }
    }
  }
}


void Horizontal_Control_VIO(uint8_t force_brake_flag)
{
  /**************************光流位置控制器************************************/
  if(OpticalFlow_Pos_Ctrl_Recode == 1 || force_brake_flag == 1)
  {
    OpticalFlow_Pos_Ctrl_Expect.x = uav_ins.position[_EAST];
    OpticalFlow_Pos_Ctrl_Expect.y = uav_ins.position[_NORTH];
    OpticalFlow_Pos_Ctrl_Recode = 0;
  }

  OpticalFlow_Pos_Control_VIO();
  /***********当只需要速度控制时，开启以下注释，仅限调试时用*************/
//	OpticalFlow_Pos_Ctrl_Output.x=0;
//	OpticalFlow_Pos_Ctrl_Output.y=0;
  /**************************基于模型的加速度-姿态角映射，相比直接给姿态，参数差异大概在20倍左右************************************/
  OpticalFlow_Vel_Control(OpticalFlow_Pos_Ctrl_Output);//速度期望
}



void OpticalFlow_X_Vel_Control(float target_x)//机头左侧为X+
{
  static uint16_t OpticalFlow_Vel_Ctrl_Cnt = 0;
  OpticalFlow_Vel_Ctrl_Expect.x = target_x; //速度期望
  OpticalFlow_Vel_Ctrl_Cnt++;

  if(OpticalFlow_Vel_Ctrl_Cnt >= 2) //10ms控制一次速度，避免输入频率过大，系统响应不过来
  {
    OpticalFlow_Vel_Ctrl_Cnt = 0;
    OpticalFlow_Vel_Ctrl_Err.x = constrain_float(OpticalFlow_Vel_Ctrl_Expect.x - uav_ins._speed[_EAST], -Total_Controller.Optical_Speed_Control.Err_Max, Total_Controller.Optical_Speed_Control.Err_Max); //30
    OpticalFlow_Vel_Ctrl_Integrate.x += Total_Controller.Optical_Speed_Control.Ki*OpticalFlow_Vel_Ctrl_Err.x; //0.1  15
    OpticalFlow_Vel_Ctrl_Integrate.x = constrain_float(OpticalFlow_Vel_Ctrl_Integrate.x, -Total_Controller.Optical_Speed_Control.Integrate_Max, Total_Controller.Optical_Speed_Control.Integrate_Max);
    OpticalFlow_Vel_Ctrl_Output.x = OpticalFlow_Vel_Ctrl_Integrate.x + Total_Controller.Optical_Speed_Control.Kp*OpticalFlow_Vel_Ctrl_Err.x; //4.5

    accel_target.x = -constrain_float(OpticalFlow_Vel_Ctrl_Output.x, -Total_Controller.Optical_Speed_Control.Control_OutPut_Limit, Total_Controller.Optical_Speed_Control.Control_OutPut_Limit); //期望运动加速度
    accel_target.y = 0;
    Desired_Accel_Transform_Angle(accel_target, &angle_target); //期望运动加速度转期望姿态倾角
    Flight.roll_outer_control_output = angle_target.x;
  }
}


void OpticalFlow_Y_Vel_Control(float target_y)//机头前侧为Y+
{
  static uint16_t OpticalFlow_Vel_Ctrl_Cnt = 0;
  OpticalFlow_Vel_Ctrl_Expect.y = target_y;
  OpticalFlow_Vel_Ctrl_Cnt++;

  if(OpticalFlow_Vel_Ctrl_Cnt >= 2) //10ms控制一次速度，避免输入频率过大，系统响应不过来
  {
    OpticalFlow_Vel_Ctrl_Cnt = 0;
    OpticalFlow_Vel_Ctrl_Err.y = constrain_float(OpticalFlow_Vel_Ctrl_Expect.y - uav_ins._speed[_NORTH], -Total_Controller.Optical_Speed_Control.Err_Max, Total_Controller.Optical_Speed_Control.Err_Max);
    OpticalFlow_Vel_Ctrl_Integrate.y += Total_Controller.Optical_Speed_Control.Ki*OpticalFlow_Vel_Ctrl_Err.y;
    OpticalFlow_Vel_Ctrl_Integrate.y = constrain_float(OpticalFlow_Vel_Ctrl_Integrate.y, -Total_Controller.Optical_Speed_Control.Integrate_Max, Total_Controller.Optical_Speed_Control.Integrate_Max);
    OpticalFlow_Vel_Ctrl_Output.y = OpticalFlow_Vel_Ctrl_Integrate.y + Total_Controller.Optical_Speed_Control.Kp*OpticalFlow_Vel_Ctrl_Err.y;

    accel_target.x = 0;
    accel_target.y = -constrain_float(OpticalFlow_Vel_Ctrl_Output.y, -Total_Controller.Optical_Speed_Control.Control_OutPut_Limit, Total_Controller.Optical_Speed_Control.Control_OutPut_Limit); //450
    Desired_Accel_Transform_Angle(accel_target, &angle_target); //期望运动加速度转期望姿态倾角
    Flight.pitch_outer_control_output = angle_target.y;
  }
}