#ifndef DJI_MOTOR_H
#define DJI_MOTOR_H

#ifdef __cplusplus
extern "C"{
#endif
#define FREERTOS_DJI 1

#include "struct_typedef.h"
#include "can.h"
#include <math.h>

#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
#define CAN_VESC_CTRL_ID_BASE (0x300)

typedef enum{
  GM6020 = 0,
  M3508 = 1,
  M2006 = 2
}motor_type_e;


//rm motor data
typedef struct
{
    uint16_t ecd;//编码器返回值0-8192
    int16_t speed_rpm;//速度
    int16_t given_current;//电流
    uint16_t temperate;//温度
    int16_t last_ecd;//上次的编码器角度
    int16_t round_cnt;//转子圈数(ecd round)
    int32_t total_angle;//输出轴角度
    uint16_t offset_ecd;//开始的ecd值
    uint32_t msg_cnt;//初始化开始ecd值用的
    int16_t real_round;//输出轴圈数
    int16_t real_angle;//输出轴单圈角度
    motor_type_e type;//电机类型，详见motor_type_e

} motor_measure_t;

typedef struct
{
    uint16_t ecd;
    int16_t speed_rpm;
    int16_t given_current;
    uint8_t temperate;
    int16_t last_ecd;
} odrive_measure_t;

/* 电机反馈信息 */
typedef struct {
  float rotor_ecd;
  float rotor_speed;
  float torque_current;
  float temp;
} CAN_MotorFeedback_t;

typedef enum {
	CAN_PACKET_SET_DUTY = 0,
	CAN_PACKET_SET_CURRENT = 1,
	CAN_PACKET_SET_CURRENT_BRAKE =2,
	CAN_PACKET_SET_RPM = 3,
	CAN_PACKET_SET_POS = 4,
	CAN_PACKET_FILL_RX_BUFFER = 5,
	CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
	CAN_PACKET_PROCESS_RX_BUFFER = 7,
	CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
	CAN_PACKET_STATUS = 9,
	CAN_PACKET_SET_CURRENT_REL = 10,
	CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
	CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
} CAN_PACKET_ID;

typedef enum {
  CAN_M3508_M1_ID = 0x201, /* 1 */
  CAN_M3508_M2_ID = 0x202, /* 2 */
  CAN_M3508_M3_ID = 0x203, /* 3 */
  CAN_M3508_M4_ID = 0x204, /* 4 */

	CAN_G6020_AgvM1 =0x205,
	CAN_G6020_AgvM2 =0x206,
	CAN_G6020_AgvM3 =0x207,
	CAN_G6020_AgvM4 =0x208,
	
	CAN_SICK_ID=0x301,
	
	CAN_G6020_Pitch =0x209,
	  
	// CAN_VESC5065_M1 =0x211,               //vesc的数据指令使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
	// CAN_VESC5065_M2 =0x212,
	// CAN_VESC5065_M3 =0x213,
	// CAN_VSEC5065_M4 =0x214,
	
  CAN_VESC5065_M1_MSG1 =0x90a,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
	CAN_VESC5065_M2_MSG1 =0x90b,

//	
} CAN_MotorId_t;

//motor calc ecd to angle 

#define MOTOR_ECD_TO_ANGLE_3508  (360.0 / 8191.0 / (3591.0/187.0))
#define MOTOR_ECD_TO_ANGLE_2006 (360.0 / 8191.0 / 36)
#define MOTOR_ECD_TO_RAD 0.000766990394f
#define MOTOR_ECD_TO_ANGLE_6020  (360.0 / 8191.0 )

#if FREERTOS_DJI == 1


/*小米电机部分参数和函数*/
//控制参数最值，谨慎更改
#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -30.0f
#define V_MAX 30.0f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -12.0f
#define T_MAX 12.0f
#define MAX_P 720
#define MIN_P -720
//主机CANID设置
#define Master_CAN_ID 0x00                      //主机ID
//控制命令宏定义
#define Communication_Type_GetID 0x00           //获取设备的ID和64位MCU唯一标识符
#define Communication_Type_MotionControl 0x01 	//用来向主机发送控制指令
#define Communication_Type_MotorRequest 0x02	//用来向主机反馈电机运行状态
#define Communication_Type_MotorEnable 0x03	    //电机使能运行
#define Communication_Type_MotorStop 0x04	    //电机停止运行
#define Communication_Type_SetPosZero 0x06	    //设置电机机械零位
#define Communication_Type_CanID 0x07	        //更改当前电机CAN_ID
#define Communication_Type_Control_Mode 0x12
#define Communication_Type_GetSingleParameter 0x11	//读取单个参数
#define Communication_Type_SetSingleParameter 0x12	//设定单个参数
#define Communication_Type_ErrorFeedback 0x15	    //故障反馈帧
//参数读取宏定义
#define Run_mode 0x7005	
#define Iq_Ref   0x7006
#define Spd_Ref  0x700A
#define Limit_Torque 0x700B
#define Cur_Kp 0x7010
#define Cur_Ki 0x7011
#define Cur_Filt_Gain 0x7014
#define Loc_Ref 0x7016
#define Limit_Spd 0x7017
#define Limit_Cur 0x7018

#define Gain_Angle 720/32767.0
#define Bias_Angle 0x8000
#define Gain_Speed 30/32767.0
#define Bias_Speed 0x8000
#define Gain_Torque 12/32767.0
#define Bias_Torque 0x8000
#define Temp_Gain   0.1

#define Motor_Error 0x00
#define Motor_OK 0X01

enum CONTROL_MODE   //控制模式定义
{
    Motion_mode = 0,//运控模式  
    Position_mode,  //位置模式
    Speed_mode,     //速度模式  
    Current_mode    //电流模式
};
enum ERROR_TAG      //错误回传对照
{
    OK                 = 0,//无故障
    BAT_LOW_ERR        = 1,//欠压故障
    OVER_CURRENT_ERR   = 2,//过流
    OVER_TEMP_ERR      = 3,//过温
    MAGNETIC_ERR       = 4,//磁编码故障
    HALL_ERR_ERR       = 5,//HALL编码故障
    NO_CALIBRATION_ERR = 6//未标定
};

typedef struct{           //小米电机结构体
	uint8_t CAN_ID;       //CAN ID
    uint8_t MCU_ID;       //MCU唯一标识符[后8位，共64位]
	float Angle;          //回传角度
	float Speed;          //回传速度
	float Torque;         //回传力矩
	float Temp;			  //回传温度
	
	uint16_t set_current;
	uint16_t set_speed;
	uint16_t set_position;
	
	uint8_t error_code;
	
	float Angle_Bias;
	
}MI_Motor;
extern MI_Motor mi_motor[4];//预先定义四个小米电机


/**
 * @brief     大疆电机初始化
 * @param     none
 * @retval    none
*/
void djiInit(void);

/**
 * @brief 等待新数据
*/
uint32_t waitNewDji(void);
#endif
/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x205) 电机控制电流
  * @param[in]      motor2: (0x206) 电机控制电流
  * @param[in]      motor3: (0x207) 电机控制电流
  * @param[in]      motor4: (0x208) 电机控制电流
  * @retval         none
  */
extern void CAN_cmd_1FF(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan);

/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 电机控制电流
  * @param[in]      motor2: (0x202) 电机控制电流
  * @param[in]      motor3: (0x203) 电机控制电流
  * @param[in]      motor4: (0x204) 电机控制电流
  * @retval         none
  */
extern void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan);

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x209) 电机控制电流
  * @param[in]      motor2: (0x20A) 电机控制电流
  * @param[in]      motor3: (0x20B) 电机控制电流
  * @retval         none
  */
extern void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan);

/**
  * @brief          返回电机数据指针
  * @param[in]      i: 电机编号
  * @retval         电机数据指针
  */
extern motor_measure_t *get_motor_point(uint8_t i);


//小米
extern void chack_cybergear(uint8_t ID);
extern void start_cybergear(MI_Motor *Motor);
extern void stop_cybergear(MI_Motor *Motor, uint8_t clear_error);
extern void set_mode_cybergear(MI_Motor *Motor, uint8_t Mode);
extern void set_current_cybergear(MI_Motor *Motor, float Current);
extern void set_zeropos_cybergear(MI_Motor *Motor);
extern void set_CANID_cybergear(MI_Motor *Motor, uint8_t CAN_ID);
extern void init_cybergear(MI_Motor *Motor, uint8_t Can_Id, uint8_t mode);
extern void motor_controlmode(MI_Motor *Motor,float torque, float MechPosition, float speed, float kp, float kd);

/**
 * @brief           数据处理函数
 * @param[in]       none
 * @retval          none
*/
void djiMotorEncode(void);
void vescMotorEncode(void);

void  CAN_VESC_RPM(uint8_t controller_id, float RPM);

void  CAN_VESC_HEAD(uint8_t controller_id);

#ifdef __cplusplus
}
#endif

#endif