/**
  ****************************(C) COPYRIGHT ZhouCc****************************
  * @file       djiMotor.c/h
  * @brief      大疆电机接收发送程序
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023.12.11			ZhouCc					完成
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT ZhouCc****************************
  */

#include "djiMotor.h"
#include "can_it.h"
#include "can_init.h"
#if FREERTOS_DJI == 1
#include "TopDefine.h"
#include<cmsis_os2.h>
#include "FreeRTOS.h"

#include "odrive_can.h"

#endif

//motor data read
#define get_motor_measure(ptr, data)                     					\
    {                                                                 	\
        (ptr)->last_ecd = (ptr)->ecd;                              			\
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6]; 																	\
				if ((ptr)->ecd - (ptr)->last_ecd > 4096)                         \
          (ptr)->round_cnt--;                                            \
        else if ((ptr)->ecd - (ptr)->last_ecd < -4096)                   \
          (ptr)->round_cnt++;																\
				if((ptr)->type == M3508)													\
					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_3508;      \
				else if((ptr)->type == M2006)																\
					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_2006;      \
				else																											\
					(ptr)->total_angle = (fp64)((ptr)->round_cnt * 8192 + (ptr)->ecd - (ptr)->offset_ecd ) * MOTOR_ECD_TO_ANGLE_6020;      \
				(ptr)->real_round = (ptr)->total_angle / 360;													\
				(ptr)->real_angle = (ptr)->total_angle % 360;													\
    }
// #define get_5065motor_measure(ptr, data)                                    \
// 		{                                                                   \
//         (ptr)->last_ecd = (ptr)->ecd;                                   \
//         (ptr)->ecd = (float)((data)[0] << 4 | (data)[1] <<3 |(data)[2] < 2 |(data)[3]);            \
//         (ptr)->speed_rpm = (float)((data)[4] << 4 | (data)[5] <<3 |(data)[6] < 2 |(data)[7]);;      \
//     }
/*(ptr)->real_angle = (ptr)->real_angle % 360;	*/
#define get_motor_offset(ptr, data)																			\
		{																																		\
				(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
				(ptr)->offset_ecd = (ptr)->ecd;																	\
		}
		
/*上下3508为0；翻转3508为1；前后2006为2；夹球2006为3，取球2006为4*/
#if DEBUG == 1
motor_measure_t motor_chassis[5];
#else
static motor_measure_t motor_chassis[5];
#endif
static CAN_TxHeaderTypeDef  tx_meessage_1ff;
static uint8_t              can_send_data_1ff[8];
static CAN_TxHeaderTypeDef  tx_meessage_2ff;
static uint8_t              can_send_data_2ff[8];
static CAN_TxHeaderTypeDef  tx_message_200;
static uint8_t              can_send_data_200[8];

CAN_RxHeaderTypeDef dji_rx_header;
uint8_t dji_rx_data[8];

/**
 * @brief           数据处理函数
 * @param[in]       none
 * @retval          none
*/
void djiMotorEncode()
{
    switch (dji_rx_header.StdId)
    {
    case 0x201:
    case 0x202:
    case 0x203:
    case 0x204:
    case 0x205:
    case 0x206:
    case 0x207:
    {
        static uint8_t i = 0;
        //get motor id
        i = dji_rx_header.StdId - 0x201;
        if(motor_chassis[i].msg_cnt<=50)
        {
            motor_chassis[i].msg_cnt++;
            get_motor_offset(&motor_chassis[i], dji_rx_data);
        }else{
            get_motor_measure(&motor_chassis[i], dji_rx_data);
        }
        break;
    }

            
    default:
    {
        break;
    }
    }
}
#if FREERTOS_DJI == 0
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
    
    djiMotorEncode();
    
}
#else
static osEventFlagsId_t eventReceive;
/**
  * @brief          自定义大疆电机回调函数
  * @param[in]      none
  * @retval         none
  */
void Dji_Motor_CB()
{
  HAL_CAN_GetRxMessage(&hcan1, CAN_RX_FIFO0, &dji_rx_header, dji_rx_data);
  
  osEventFlagsSet(eventReceive, EVENT_CAN);
}

/**
 * @brief     大疆电机初始化
 * @param     none
 * @retval    none
*/
void djiInit(void)
{
  BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
                          Dji_Motor_CB);
  can_filter_init();
}

/**
 * @brief 等待新数据
*/
uint32_t waitNewDji() 
{
  return osEventFlagsWait(
				eventReceive, EVENT_CAN,osFlagsWaitAny, osWaitForever);
}
#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)
{
    uint32_t send_mail_box;
    tx_meessage_1ff.StdId = 0x1FF;
    tx_meessage_1ff.IDE = CAN_ID_STD;
    tx_meessage_1ff.RTR = CAN_RTR_DATA;
    tx_meessage_1ff.DLC = 0x08;
    can_send_data_1ff[0] = (motor1 >> 8);
    can_send_data_1ff[1] = motor1;
    can_send_data_1ff[2] = (motor2 >> 8);
    can_send_data_1ff[3] = motor2;
    can_send_data_1ff[4] = (motor3 >> 8);
    can_send_data_1ff[5] = motor3;
    can_send_data_1ff[6] = (motor4 >> 8);
    can_send_data_1ff[7] = motor4;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_1ff, can_send_data_1ff, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x201,0x202,0x203,0x204)
  * @param[in]      motor1: (0x201) 电机控制电流
  * @param[in]      motor2: (0x202) 电机控制电流
  * @param[in]      motor3: (0x203) 电机控制电流
  * @param[in]      motor4: (0x204) 电机控制电流
  * @retval         none
  */
void CAN_cmd_200(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4,CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_message_200.StdId = 0x200;
    tx_message_200.IDE = CAN_ID_STD;
    tx_message_200.RTR = CAN_RTR_DATA;
    tx_message_200.DLC = 0x08;
    can_send_data_200[0] = motor1 >> 8;
    can_send_data_200[1] = motor1;
    can_send_data_200[2] = motor2 >> 8;
    can_send_data_200[3] = motor2;
    can_send_data_200[4] = motor3 >> 8;
    can_send_data_200[5] = motor3;
    can_send_data_200[6] = motor4 >> 8;
    can_send_data_200[7] = motor4;

    HAL_CAN_AddTxMessage(hcan, &tx_message_200, can_send_data_200, &send_mail_box);
}

/**
  * @brief          发送电机控制电流(0x205,0x206,0x207,0x208)
  * @param[in]      motor1: (0x209) 电机控制电流
  * @param[in]      motor2: (0x20A) 电机控制电流
  * @param[in]      motor3: (0x20B) 电机控制电流
  * @retval         none
  */
void CAN_cmd_2FF(int16_t motor1, int16_t motor2, int16_t motor3, CAN_HandleTypeDef*hcan)
{
    uint32_t send_mail_box;
    tx_meessage_2ff.StdId = 0x1FF;
    tx_meessage_2ff.IDE = CAN_ID_STD;
    tx_meessage_2ff.RTR = CAN_RTR_DATA;
    tx_meessage_2ff.DLC = 0x08;
    can_send_data_2ff[0] = (motor1 >> 8);
    can_send_data_2ff[1] = motor1;
    can_send_data_2ff[2] = (motor2 >> 8);
    can_send_data_2ff[3] = motor2;
    can_send_data_2ff[4] = (motor3 >> 8);
    can_send_data_2ff[5] = motor3;
    can_send_data_2ff[6] = (0 >> 8);
    can_send_data_2ff[7] = 0;
    HAL_CAN_AddTxMessage(hcan, &tx_meessage_2ff, can_send_data_2ff, &send_mail_box);
}

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