#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

/**
 * @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);


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

void  CAN_VESC_RPM(uint8_t controller_id, float RPM);

#ifdef __cplusplus
}
#endif

#endif
