/*
 * 云台模组
 模式自行选择：遥控器改变或自行初始化
 delta获取从遥控器自行获取
 example:
	Gimbal_t gimbal;
	Gimbal_IMU_t gimbal_imu;
	Gimbal_CMD_t gimbal_cmd;

	Gimbal_Init(&gimbal,&Config_GetRobotParam()->gimbal_param,GIMBAL_CTRL_FREQ);
 
	while(1){
	//这里写遥控器获取gimbal_cmd数据
		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0))
    Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
		
 		Gimbal_UpdateFeedback(&gimbal);
    Gimbal_Control(&gimbal,&gimbal_cmd);
    Gimbal_Output(&gimbal);
		}
 */

/* Includes ----------------------------------------------------------------- */
#include "gimbal.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "component/pid.h"
#include "device/motor_rm.h"
#include "device/motor_dm.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Private function  -------------------------------------------------------- */
/**
 * \brief 计算角度偏差
 *
 * \param 
 * \param  
 *
 * \return 函数运行结果
 */
static float motor_imu_offset(float* motor, float* imu){

	float motor_imu_offset = motor - imu;
    /* 处理跨越±π的情况 */
   if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
   if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;

	return motor_imu_offset;
}




/**
 * \brief 设置云台模式
 *
 * \param c 包含云台数据的结构体
 * \param mode 要设置的模式
 *
 * \return 函数运行结果
 */
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
  if (g == NULL)
    return -1;
  if (mode == g->mode)
    return GIMBAL_OK;
	SMC_Reset(&g->smc.yaw);
	SMC_Reset(&g->smc.pit);
	
  LowPassFilter2p_Reset(&g->filter_out.major_yaw, 0.0f);	
  LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
  LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);

  LowPassFilter2p_Reset(&g->filter_out.gyro_x, 0.0f);	
  LowPassFilter2p_Reset(&g->filter_out.gyro_y, 0.0f);
  LowPassFilter2p_Reset(&g->filter_out.gyro_z, 0.0f);

  
  AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
	
  g->setpoint.eulr.pit = g->feedback.imu.eulr.pit;
  g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;


  g->mode = mode;
  return 0;
}

/* Exported functions ------------------------------------------------------- */

/**
 * \brief 初始化云台
 *
 * \param g 包含云台数据的结构体
 * \param param 包含云台参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
                   float target_freq) {
  if (g == NULL)
    return -1;

  g->param = param;
  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */

  /* 初始化云台电机控制滑膜 */

	SMC_Init(&g->smc.pit,EISMC,&g->param->smc.pit);
	SMC_Init(&g->smc.yaw,EXPONENT,&g->param->smc.yaw);	

	g->limit.yaw.max = g->param->Limit_t.zero.yaw_encoder+g->param->Limit_t.zero.travel.yaw;			 
	g->limit.yaw.min = g->param->Limit_t.zero.yaw_encoder;

  LowPassFilter2p_Init(&g->filter_out.major_yaw, target_freq,
                       g->param->low_pass_cutoff_freq.out);	
	
  LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
                       g->param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
                       g->param->low_pass_cutoff_freq.out);
	/* 陀螺仪角速度反馈滤波 */
  LowPassFilter2p_Init(&g->filter_out.gyro_x, target_freq,
                       g->param->low_pass_cutoff_freq.out);	
  LowPassFilter2p_Init(&g->filter_out.gyro_y, target_freq,
                       g->param->low_pass_cutoff_freq.out);
  LowPassFilter2p_Init(&g->filter_out.gyro_z, target_freq,
                       g->param->low_pass_cutoff_freq.out);
  BSP_CAN_Init();
	/* 大yaw电机注册 */
	if(g->param->motor.major_yaw==RM)
			MOTOR_RM_Register(&(g->param->motor.major_yaw_rm_motor));	
	
	if(g->param->motor.major_yaw==DM){
			MOTOR_DM_Register(&(g->param->motor.major_yaw_dm_motor));
			MOTOR_DM_Enable(&(g->param->motor.major_yaw_dm_motor));
	}	
	
	/*大疆电机注册*/
	if(g->param->motor.yaw==RM)
			MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
	if(g->param->motor.pit==RM)
			MOTOR_RM_Register(&(g->param->motor.pit_rm_motor));
	
	/*达妙电机注册*/
	if(g->param->motor.yaw==DM){
			MOTOR_DM_Register(&(g->param->motor.yaw_dm_motor));
			MOTOR_DM_Enable(&(g->param->motor.yaw_dm_motor));
	}
	
	if(g->param->motor.pit==DM){
			MOTOR_DM_Register(&(g->param->motor.pit_dm_motor));
			MOTOR_DM_Enable(&(g->param->motor.pit_dm_motor));
	}
	
  return 0;
}
static int8_t Gimbal_Direction(Gimbal_t *g){

  if (g == NULL) {
    return -1;
  }
	/* 欧拉角方向选择 */
	switch(g->param->Direction.Eulr.pit){
		case Pit:
			g->direction.Eulr.pit=g->feedback.imu.eulr.pit;
			break;
		case Yaw:
			g->direction.Eulr.pit=g->feedback.imu.eulr.yaw;
			break;	
		case Rol:
			g->direction.Eulr.pit=g->feedback.imu.eulr.rol;
			break;	
	}
	
	switch(g->param->Direction.Eulr.yaw){
		case Pit:
			g->direction.Eulr.yaw=g->feedback.imu.eulr.pit;
			break;
		case Yaw:
			g->direction.Eulr.yaw=g->feedback.imu.eulr.yaw;
			break;	
		case Rol:
			g->direction.Eulr.yaw=g->feedback.imu.eulr.rol;
			break;	
	}	
	/* 角速度方向选择 */
	switch(g->param->Direction.Gyro.pit){
		case Gyro_x:
			g->direction.Gyro.pit=g->feedback.imu.gyro.x;
			break;
		case Gyro_y:
			g->direction.Gyro.pit=g->feedback.imu.gyro.y;
			break;	
		case Gyro_z:
			g->direction.Gyro.pit=g->feedback.imu.gyro.z;
			break;	
	}
	
	switch(g->param->Direction.Gyro.yaw){
		case Gyro_x:
			g->direction.Gyro.yaw=g->feedback.imu.gyro.x;
			break;
		case Gyro_y:
			g->direction.Gyro.yaw=g->feedback.imu.gyro.y;
			break;	
		case Gyro_z:
			g->direction.Gyro.yaw=g->feedback.imu.gyro.z;
			break;	
	}
    return 0;
}
/**
 * \brief 通过CAN设备更新云台反馈信息
 *
 * \param gimbal 云台
 * \param can CAN设备
 *
 * \return 函数运行结果
 */
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
  if (gimbal == NULL)
    return -1;

	
	  /* 更新大yaw电机反馈数据（RM和DM） */	
	if(gimbal->param->motor.major_yaw==RM){
		MOTOR_RM_Update(&(gimbal->param->motor.major_yaw_rm_motor));
		MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.major_yaw_rm_motor));
		if(rm_motor_yaw != NULL)
		 gimbal->feedback.motor.major_yaw = rm_motor_yaw->feedback;
	}	
	
	if(gimbal->param->motor.major_yaw==DM){
		MOTOR_DM_Update(&(gimbal->param->motor.major_yaw_dm_motor));
		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.major_yaw_dm_motor));
			if (dm_motor != NULL) {
				gimbal->feedback.motor.major_yaw = dm_motor->motor.feedback;
		}
	}	
	
  /* 更新RM电机反馈数据 */
	if(gimbal->param->motor.yaw==RM){
		MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor));
		MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.yaw_rm_motor));
		if(rm_motor_yaw != NULL)
		 gimbal->feedback.motor.yaw = rm_motor_yaw->feedback;
	}
	
	
	if(gimbal->param->motor.pit==RM){
	MOTOR_RM_Update(&(gimbal->param->motor.pit_rm_motor));
	MOTOR_RM_t *rm_motor_pit = MOTOR_RM_GetMotor(&(gimbal->param->motor.pit_rm_motor));
	if (rm_motor_pit != NULL)
		gimbal->feedback.motor.pit = rm_motor_pit->feedback;
	}
	
	  /* 更新DM电机反馈数据 */
	if(gimbal->param->motor.yaw==DM){
		MOTOR_DM_Update(&(gimbal->param->motor.yaw_dm_motor));
		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.yaw_dm_motor));
			if (dm_motor != NULL) {
				gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
		}
	}
	if(gimbal->param->motor.pit==DM){
		MOTOR_DM_Update(&(gimbal->param->motor.pit_dm_motor));
		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.pit_dm_motor));
			if (dm_motor != NULL) {
				gimbal->feedback.motor.pit = dm_motor->motor.feedback;
		}
	}

  return 0;
}

int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){

  if (gimbal == NULL) {
    return -1;
  }
  gimbal->feedback.imu.gyro = imu->gyro;
  gimbal->feedback.imu.eulr = imu->eulr;
	Gimbal_Direction(gimbal);
}
/**
 * \brief 运行大yaw控制逻辑
 *
 * \param g 包含云台数据的结构体
 * \param 
 *
 * \return 函数运行结果
 */
static int8_t major_yaw_Control(Gimbal_t *g){

	 /*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
 /*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
 跟随小YAW向相同方向运动，将遥控数据进行分配*/

 /*小YAW中点*/
 float small_yaw_center_offset = g->param->Limit_t.zero.yaw_encoder  + 
								 g->param->Limit_t.zero.travel.yaw* 0.5f;
	g->setpoint.major_yaw=small_yaw_center_offset;
	return 0;
}


/**
 * \brief 运行云台控制逻辑
 *
 * \param g 包含云台数据的结构体
 * \param g_cmd 云台控制指令
 *
 * \return 函数运行结果
 */
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
  if (g == NULL || g_cmd == NULL) {
    return -1;
  }

	
  g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
  g->lask_wakeup = BSP_TIME_Get_us();
  Gimbal_SetMode(g, g_cmd->mode);
	/* 双yaw的大yaw,setpoint从这设立 */
	major_yaw_Control(g);
  /* 欧拉角控制相关逻辑 */
  float yaw_omega_set_point, pit_omega_set_point,major_yaw_omega_set_point;
	
	float delta_yaw = g_cmd->delta_yaw*g->dt;
	float delta_pit = g_cmd->delta_pit*g->dt;

		float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,
			&g->feedback.imu.eulr.yaw);

    float delta_max = CircleError(g->limit.yaw.max,
        (g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);
    float delta_min = CircleError(g->limit.yaw.min,
        (g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);		
		
		/*角速度滤波*/
   g->feedback.imu.gyro.x = LowPassFilter2p_Apply(&g->filter_out.gyro_x, g->feedback.imu.gyro.x);
   g->feedback.imu.gyro.y = LowPassFilter2p_Apply(&g->filter_out.gyro_y, g->feedback.imu.gyro.y);
   g->feedback.imu.gyro.z = LowPassFilter2p_Apply(&g->filter_out.gyro_z, g->feedback.imu.gyro.z);		
		
  switch (g->mode) {
  case GIMBAL_MODE_RELAX:/*放松模式*/
    g->out.yaw = 0.0f;
    g->out.pit = 0.0f;
    break;
  case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
	
		g->setpoint.eulr.yaw+=delta_yaw;	

		if (g->param->motor.limit_yaw==true)
		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);		
		else{
		/*限制在-3.14~3.14*/
		if (g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
		}

  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
		g->setpoint.eulr.pit+=delta_pit;
	  /* 限制pit控制命令 */	
		if (g->param->motor.limit_pit == true)
		Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);	
 /*4310大YAW控制
    这里是单环控制的，有需要加双环
    */

		/* 这两条仅用于电机测试 */																			
//	 SMC_PErrorUpdate( &g->smc.yaw,
//											g->setpoint.eulr.yaw,
//											g->feedback.motor.yaw.rotor_abs_angle,
//											g->feedback.motor.yaw_omega*0.002,g->dt);

//	 SMC_PErrorUpdate( &g->smc.pit,
//											g->setpoint.eulr.pit,
//											g->feedback.motor.pit.rotor_abs_angle,
//											g->feedback.motor.pit_omega*0.002,g->dt);		

	 SMC_PErrorUpdate(&g->smc.yaw,
										 g->setpoint.eulr.yaw,
										 g->feedback.imu.eulr.yaw,
										 g->feedback.imu.gyro.z*0.002,g->dt);
	 SMC_PErrorUpdate(&g->smc.pit,
										 g->setpoint.eulr.pit,
										 g->feedback.imu.eulr.pit,
										 g->feedback.imu.gyro.x*0.002,g->dt);										 
		g->out.yaw=Smc_Calc(&g->smc.yaw);
		g->out.pit=Smc_Calc(&g->smc.pit);
	
		/*前馈添加*/
		if(g->param->feedforward.imu.yaw==true)
		g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
		if(g->param->feedforward.imu.pit==true)
		g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
		
    break;
	case GIMBAL_MODE_RELATIVE:
		/*计算零点*/
//		g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
//		g->zero.travel.pit=motor_imu_offset(&g->feedback.motor.pit.rotor_abs_angle,&g->feedback.imu.eulr.pit);	
			
//		g->zero.yaw = g->param->zero.yaw_encoder - g->zero.travel.yaw;
//		g->zero.pit = g->param->zero.pit_encoder - g->zero.travel.pit;	
		/*基于零点的设定角度*/
		/*加的相对角度应该限制在3.14~-3.14*/
		CircleAdd(&(g->relative_angle.yaw),delta_yaw,M_2PI);
		if((g->relative_angle.yaw)>=M_2PI)g->relative_angle.yaw-=M_2PI;
		/*限制pit的累加角度*/
		g->relative_angle.pit=+delta_pit;
		Clip(&(g->relative_angle.pit),(g->param->Limit_t.pit_min+g->zero.pit),
		(g->param->Limit_t.pit_max-g->zero.pit));
	
		g->setpoint.eulr.yaw=g->zero.yaw + g->relative_angle.yaw;
		g->setpoint.eulr.pit=g->zero.pit + g->relative_angle.pit;

		/*yaw的限位*/	
		if (g->param->motor.limit_yaw==true)
		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
		else{
		/*限制在-3.14~3.14*/
		if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
		}
		/* pit限制控制命令 */
		if (g->param->motor.limit_pit==true)
		Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);		

				
		/*前馈添加*/
		if(g->param->feedforward.imu.yaw==true)
		g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
		if(g->param->feedforward.imu.pit==true)
		g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
		break;
  }
	    /* 输出滤波 */
    g->out.major_yaw = LowPassFilter2p_Apply(&g->filter_out.major_yaw, g->out.major_yaw);	
    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
	 return 0;
}





  /**
   * \brief 云台输出
   *
   * \param s 包含云台数据的结构体
   * \param out CAN设备云台输出结构体
   */
void Gimbal_Output(Gimbal_t *g){
	
	/*大yaw电机输出*/
	if(g->param->motor.major_yaw==RM){
			MOTOR_RM_Ctrl(&g->param->motor.major_yaw_rm_motor);
			MOTOR_RM_SetOutput(&g->param->motor.major_yaw_rm_motor, g->out.major_yaw);
	}	

	if(g->param->motor.major_yaw==DM){
		
			g->out.major_yaw_dm.angle=g->param->dm_Params_t.major_yaw_dm.angle;
			g->out.major_yaw_dm.kd=g->param->dm_Params_t.major_yaw_dm.kd;
			g->out.major_yaw_dm.kp=g->param->dm_Params_t.major_yaw_dm.kp;
			g->out.major_yaw_dm.velocity=g->param->dm_Params_t.major_yaw_dm.velocity;
		
			g->out.major_yaw_dm.torque= g->out.pit * g->param->dm_Params_t.major_yaw_dm_Reduction_ratio; // 乘以减速比
		
			 MOTOR_DM_MITCtrl(&g->param->motor.major_yaw_dm_motor,&(g->out.major_yaw_dm));
	}	
	
	/*大疆电机输出*/
	if(g->param->motor.yaw==RM){
			MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);
			MOTOR_RM_SetOutput(&g->param->motor.yaw_rm_motor, g->out.yaw);
	}
	if(g->param->motor.pit==RM){
			MOTOR_RM_Ctrl(&g->param->motor.pit_rm_motor);
			MOTOR_RM_SetOutput(&g->param->motor.pit_rm_motor, g->out.pit);//
	}
	
	/*达妙电机输出*/
	if(g->param->motor.pit==DM){
		
			g->out.pit_dm.angle=g->param->dm_Params_t.pit_dm.angle;
			g->out.pit_dm.kd=g->param->dm_Params_t.pit_dm.kd;
			g->out.pit_dm.kp=g->param->dm_Params_t.pit_dm.kp;
			g->out.pit_dm.velocity=g->param->dm_Params_t.pit_dm.velocity;
		
			g->out.pit_dm.torque= g->out.pit * g->param->dm_Params_t.pit_dm_Reduction_ratio; // 乘以减速比
		
			 MOTOR_DM_MITCtrl(&g->param->motor.pit_dm_motor,&(g->out.pit_dm));
	}
	if(g->param->motor.yaw==DM){
			g->out.yaw_dm.angle=g->param->dm_Params_t.yaw_dm.angle;
			g->out.yaw_dm.kd=g->param->dm_Params_t.yaw_dm.kd;
			g->out.yaw_dm.kp=g->param->dm_Params_t.yaw_dm.kp;
			g->out.yaw_dm.velocity=g->param->dm_Params_t.yaw_dm.velocity;

			g->out.yaw_dm.torque= g->out.yaw * g->param->dm_Params_t.yaw_dm_Reduction_ratio; // 乘以减速比
		
			 MOTOR_DM_MITCtrl(&g->param->motor.yaw_dm_motor,&(g->out.yaw_dm));	
		}
	
}