#ifndef DJI_MOTOR_H
#define DJI_MOTOR_H

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

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


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

typedef union{
  int as_array;
  struct{
  int8_t byte1;
  int8_t byte2;
  int8_t byte3;
  int8_t byte4;
  }byte;
}Vesc_ByteGet;

typedef struct {
  CAN_TxHeaderTypeDef tx_header;
  uint8_t tx_data[8];
} CAN_RawTx_t;

enum{
  M3508_1 = 0x201,
  M3508_2 = 0x202,
  M3508_3 = 0x203,
  M3508_4 = 0x204,
  M2006_1 = 0x205,
  M2006_2 = 0x206,
  GM6020_1 = 0x207,
  GM6020_2 = 0x208,
  CAN_VESC5065_M1_MSG1 =0x901,           //vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
	CAN_VESC5065_M2_MSG1 =0x902,
};

//vesc指令
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;

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

#define wtrcfg_VESC_COMMAND_ERPM_MAX 55000 //限速
#define CAN_VESC_CTRL_ID_BASE (0x300)

#define CAN_MOTOR_TX_BUF_SIZE (8)
#define CAN_MOTOR_RX_BUF_SIZE (8)

//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;


//motor calc ecd to angle 
#define MOTOR_ECD_TO_ANGLE_3508  (360.0 / 8191.0 / (3591.0/187.0))
#define MOTOR_ECD_TO_ANGLE_6020  (360.0 / 8191.0 )
#define MOTOR_ECD_TO_ANGLE_2006  (360.0 / 8191.0 / 36)
#define CAN_MOTOR_ENC_RES (8191)  /* 电机编码器分辨率 */
#define M_2PI 6.28318530717958647692f
#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);

 extern CAN_MotorFeedback_t *get_vesc_point(uint8_t i);
/**
 * @brief           数据处理函数
 * @param[in]       none
 * @retval          none
*/
void djiMotorEncode(void);

void can2MotorEncode(void);

void  CAN_VESC_RPM(uint8_t controller_id, float RPM);

void  CAN_VESC_HEAD(uint8_t controller_id);

int8_t CAN_VESC_Control(int id,int speed ,CAN_HandleTypeDef*hcan);

#ifdef __cplusplus
}
#endif

#endif
