/*云台控制代码*/

/*
云台模组

example：

Gimbal_CMD_t cmd_gimbal;
Gimbal_IMU_t gimbal_imu;
Gimbal_t gimbal;

Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal, GIMBAL_FREQ);

while(1)
{
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu,NULL, 0);
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);

osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0);
Gimbal_UpdateFeedback(&gimbal);

osMessageQueueReset(task_runtime.msgq.gimbal.yaw6020);
osMessageQueuePut(task_runtime.msgq.gimbal.yaw6020, &gimbal.feedback.motor.yaw_4310_motor_feedback, 0, 0);

Gimbal_Control(&gimbal, &cmd_gimbal);
Gimbal_Output(&gimbal);

}

后续可以加入NUC传来的数据命令
*/


#include "gimbal.h"
#include "device/motor_dm.h"
#include "component/pid.h"
#include "bsp/can.h"
#include "bsp/time.h"
#include "component/filter.h"
#include "math.h"
#include "module/cmd.h"
#include "device/motor_rm.h"
#include "device/motor.h"


/*
  *拟合函数，用于对pitch的重力补偿
*/
static double poly(double x) {
    return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984;
}



/**
 * \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;
  
    PID_Reset(&g->pid.yaw_4310_angle);
    PID_Reset(&g->pid.yaw_4310_omega);
    PID_Reset(&g->pid.pitch_4310_angle);
    PID_Reset(&g->pid.pitch_4310_omega);
    PID_Reset(&g->pid.yaw_6020_angle);
    PID_Reset(&g->pid.yaw_6020_omega);
    LowPassFilter2p_Reset(&g->filter_out.yaw_4310,0.0f);
    LowPassFilter2p_Reset(&g->filter_out.pitch_4310,0.0f);
    LowPassFilter2p_Reset(&g->filter_out.yaw_6020,0.0f);
    MOTOR_DM_Enable(&g->param->yaw_4310_motor);
    MOTOR_DM_Enable(&g->param->pitch_4310_motor);

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

    g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
    g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;

    g->mode = mode;
    return GIMBAL_OK;

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

/**
 * \brief 初始化云台
 *
 * \param g 包含云台数据的结构体
 * \param param 包含云台参数的结构体指针
 * \param target_freq 任务预期的运行频率
 *
 * \return 函数运行结果
 */

/*云台初始化内容*/
int8_t Gimbal_Init(Gimbal_t *g,  Gimbal_Param_t *param,float target_freq)
{
  if(g == NULL || param == NULL)
  {
      return GIMBAL_ERR; // 参数错误
  }
  g->param = param;
  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/
  /*6020小yaw pid初始化*/
  PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle );
  PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega );
  /*4310大yaw pid初始化 大yaw单环控*/
  PID_Init(&g->pid.yaw_4310_angle,KPID_MODE_SET_D, target_freq,&g->param->pid.yaw_4310_motor_angle );
  /*4310 pitch pid初始化 单环*/
  PID_Init(&g->pid.pitch_4310_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.pitch_4310_motor_angle );
  PID_Init(&g->pid.pitch_4310_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.pitch_4310_motor_omega );
  /*输出滤波器初始化*/
  LowPassFilter2p_Init(&g->filter_out.yaw_6020, target_freq, g->param->low_pass_cutoff_freq.out);

  /*机械限位*/
  g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020;
  g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ;
  g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310;
  g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ;

  BSP_CAN_Init();
  MOTOR_DM_Register(&g->param->yaw_4310_motor);
  MOTOR_DM_Register(&g->param->pitch_4310_motor);
  MOTOR_RM_Register(&g->param->yaw_6020_motor);
  MOTOR_DM_Enable(&g->param->yaw_4310_motor);
  MOTOR_DM_Enable(&g->param->pitch_4310_motor);
  return GIMBAL_OK;
}

 /**
 * \brief 通过CAN设备更新云台电机反馈信息
 *
 * \param gimbal 云台
 * \param can CAN设备
 *
 * \return 函数运行结果
 */

int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal)
{
  if (gimbal == NULL)
    return GIMBAL_ERR_NULL;
  MOTOR_RM_UpdateAll();
  MOTOR_RM_Update(&gimbal->param->yaw_6020_motor);
  gimbal->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&(MOTOR_RM_GetMotor(&gimbal->param->yaw_6020_motor)->motor));
  gimbal->feedback.motor.yaw_6020_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&(MOTOR_RM_GetMotor(&gimbal->param->yaw_6020_motor)->motor));  
  
  MOTOR_DM_Update(&gimbal->param->yaw_4310_motor);
  MOTOR_DM_t *yaw_4310_motor = MOTOR_DM_GetMotor(&gimbal->param->yaw_4310_motor);
  if(yaw_4310_motor != NULL)
  {
      gimbal->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&yaw_4310_motor->motor);
      gimbal->feedback.motor.yaw_4310_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&yaw_4310_motor->motor);

  }

  MOTOR_DM_Update(&gimbal->param->pitch_4310_motor);
  MOTOR_DM_t *pitch_4310_motor = MOTOR_DM_GetMotor(&gimbal->param->pitch_4310_motor);
  if(pitch_4310_motor != NULL)
  {
      gimbal->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle = MOTOR_GetRotorAbsAngle(&pitch_4310_motor->motor);
      gimbal->feedback.motor.pitch_4310_motor_feedback.rotor_speed = MOTOR_GetRotorSpeed(&pitch_4310_motor->motor);
    
  }

  return GIMBAL_OK;
}

/*
*更新陀螺仪数据欧拉角的值
*/

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

  if (gimbal == NULL) {
    return GIMBAL_ERR; // 参数错误
  }
  gimbal->feedback.imu.gyro = imu->gyro;

  gimbal->feedback.imu.eulr = imu->eulr;
  gimbal->feedback.imu.eulr.pit = -imu->eulr.rol; // 坐标系转换
  return GIMBAL_OK;
}

/**
 * \brief 运行云台控制逻辑
 *
 * \param g 包含云台数据的结构体
 * \param g_cmd 云台控制指令
 *
 * \return 函数运行结果
 */
// float motor_imu_offset;
//  float delta_max_pitch;
//  float delta_min_pitch;
/*这三个参数是用来调pitch的*/
 
 int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd)
{
    if (g == NULL || g_cmd == NULL)
    {
        return GIMBAL_ERR; // 参数错误
    }

  g->dt = (BSP_TIME_Get_us() - g->last_wakeup) / 1000000.0f;
  g->last_wakeup = BSP_TIME_Get_us();

 Gimbal_SetMode(g, g_cmd->mode);

/*处理小yaw轴控制命令*/
float delta_yaw_6020 = 3.0f*g_cmd->delta_yaw_6020 * g->dt;

if(g->param->travel.yaw_6020 > 0)  // 有限位才处理
{
/* 计算当前电机角度与IMU角度的偏差 */
float motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI;
if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI;
/*计算限位距离*/
const float delta_max_6020 = CircleError(g->limit.yaw_6020.max,
    (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);
const float delta_min_6020 = CircleError(g->limit.yaw_6020.min,
    (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI);    

    if(delta_yaw_6020 > delta_max_6020) delta_yaw_6020 = delta_max_6020;
    if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020;
}
  CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI);

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

 /*小YAW中点*/
 float small_yaw_center_offset = g->param->mech_zero.yaw_6020  + g->param->travel.yaw_6020* 0.5f ;
 g->setpoint.yaw_4310=small_yaw_center_offset;

  /*处理大pitch控制命令*/
float delta_pitch_4310 = 3.0f*g_cmd->delta_pitch_4310*g->dt;
if(g->param->travel.pitch_4310 > 0)  // 有限位才处理
{
   /* 计算当前电机角度与IMU角度的偏差 */
  float  motor_imu_offset = g ->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.rol;
    /* 处理跨越±π的情况 */
    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
    
    /* 计算到限位边界的距离 */
    // const float delta_max = CircleError(g->limit.pitch_4310.max,
    //     (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI);
    // const float delta_min = CircleError(g->limit.pitch_4310.min,
    //     (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI);
      float delta_max_pitch = CircleError(g->limit.pitch_4310.max,
        (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI);
    float delta_min_pitch = CircleError(g->limit.pitch_4310.min,
        (g->setpoint.eulr.pit + motor_imu_offset + delta_pitch_4310), M_2PI);
    
    /* 限制控制命令 */
    if (delta_pitch_4310 > delta_max_pitch) delta_pitch_4310 = delta_max_pitch;
    if (delta_pitch_4310 < delta_min_pitch) delta_pitch_4310 = delta_min_pitch;
}
  CircleAdd(&(g->setpoint.eulr.pit), delta_pitch_4310, M_2PI);

/* 控制相关逻辑 */
  float yaw_omega_set_point, pitch_omega_set_point,yaw_6020_omega_set_point;
switch (g->mode) {
  case GIMBAL_MODE_RELAX:
    g->out.yaw_4310 = 0.0f;
    g->out.pitch_4310 = 0.0f;
    g->out.yaw_6020 = 0.0f;
    break;
    case GIMBAL_MODE_ABSOLUTE:
    /*绝对坐标   大地坐标系下云台控制*/
    /*6020小YAW控制*/
    yaw_6020_omega_set_point =PID_Calc(&g->pid.yaw_6020_angle,g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw,0.0f,g->dt);
    g->out.yaw_6020 = PID_Calc(&g->pid.yaw_6020_omega,yaw_6020_omega_set_point,
                                  g->feedback.imu.gyro.z,0.0f,g->dt);  
    /*4310大YAW控制
    这里是单环控制的，有需要加双环
    */
    yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
                                g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
    // g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
    //                           g->feedback.imu.gyro.z,0.0f,g->dt);
    g->out.yaw_4310 = yaw_omega_set_point;  // 直接输出速度环目标值作为电机输出
      /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
    pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
                                    g->feedback.imu.eulr.rol,0.0f,g->dt);

    g->out.pitch_4310 = pitch_omega_set_point+poly(g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle);

                          

    break;
/*云台相对底盘控制，暂时没写，后面可以更进*/
    case GIMBAL_MODE_RELATIVE:
    g->out.yaw_4310 = 0.0f;
    g->out.pitch_4310 = 0.0f;
    g->out.yaw_6020 = 0.0f;
    break;

    /* 6020输出滤波 4310个人觉得不是很需要滤波，可以自己加*/
    g->out.yaw_6020 = LowPassFilter2p_Apply(&g->filter_out.yaw_6020, g->out.yaw_6020);
    
    
}
return 0;
}

/**
   * \brief 云台输出
   *
   * \param g 包含云台数据的结构体
   * \param out CAN设备云台输出结构体
   * void类型的，可以改成int  
   * 4310为力矩输出控制，传统的电机控制方法，两者之积不要大于12.5
   */

void Gimbal_Output(Gimbal_t *g)
{
  MOTOR_RM_SetOutput(&g->param->yaw_6020_motor, g->out.yaw_6020);
  MOTOR_MIT_Output_t output_yaw_4310 = {0};
    output_yaw_4310.torque = g->out.yaw_4310 * 1.0f;

    MOTOR_MIT_Output_t output_pitch_4310 = {0};
    output_pitch_4310.torque = g->out.pitch_4310 * 1.0f;
    output_pitch_4310.kd=0.2f;

    MOTOR_RM_Ctrl(&g->param->yaw_6020_motor);
    MOTOR_DM_MITCtrl(&g->param->yaw_4310_motor, &output_yaw_4310);
    MOTOR_DM_MITCtrl(&g->param->pitch_4310_motor, &output_pitch_4310);
}













