#include "bsp.h"
#include "tim.h"
#include "pwm.h"


/* USER CODE BEGIN 0 */
int16_t Current[Motion_Num]={0};
uint8_t   RTK_Send_Flag=0;
static uint16_t   time_sharing=0;
/* USER CODE END 0 */
static uint8_t PWM_FLAG=0;//PWM执行一次标志位
static PWM_StateTypeDef pwm_state = PWM_STATE_DISABLED;  // 当前PWM状态
static uint32_t startup_delay_tick = 0;                  // 启动延迟计时
/* USER CODE BEGIN 1 */
LOOPRXTYPE  U_RX_BUFFER[Usart_Num];
/* USER CODE END 1 */

/* USER CODE BEGIN 2 */
uint8_t SBUS_DATA[SBUS_SINGLE_BUFSIZE]={0};
_NOM Re_Bool={ 
    .Bit.bool_0=0,  
    .Bit.bool_1=0,  
    .Bit.bool_2=0,  
    .Bit.bool_3=0,  
    .Bit.bool_4=0,  
    .Bit.bool_5=0,  
    .Bit.bool_6=0,  
    .Bit.bool_7=0   
 };

SBUS_CH_Struct SBUS_CH;
uint8_t sbus_init_flag = 0;
/* USER CODE END 2 */

/* USER CODE RECEIVE CAN */
/*
* 函数名 :CAN_Rx
* 描述 : can中断接收函数
* 根据CAN 具体配置 改写下方配置函数以及调用；
    *  0：无错误
    *  1：尚未学习
    *  2：堵转停止
    *  3：霍尔错误
    *  4：无法达到目标速度
    *  5：保留
    *  6：过流
    *  7：过热
    *  8：过压
    *  9：欠压
    *  10：短路/过流
*/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RXBuffer CAN1_RX_buffer;
    CAN_RXBuffer CAN2_RX_buffer;
    uint8_t   CAN2_RX_R_data[8],CAN2_RX_L_data[8]={0};
    _uint2B Data_Move2; 
    
    if(HAL_CAN_GetRxFifoFillLevel(hcan,CAN_RX_FIFO0)!=1)//有数据
    {
    }
        if(hcan->Instance==hcan1.Instance)
        {
            HAL_CAN_GetRxMessage(hcan, CAN_FILTER_FIFO0, &CAN1_RX_buffer.RXHeader, CAN1_RX_buffer.DATA);//获取数据
            
        }

        if(hcan->Instance==hcan2.Instance)
        {
            HAL_CAN_GetRxMessage(hcan, CAN_FILTER_FIFO0, &CAN2_RX_buffer.RXHeader, CAN2_RX_buffer.DATA);//获取数据
            
            if(CAN2_RX_buffer.RXHeader.StdId == 0x582)
            {
                memcpy(CAN2_RX_R_data,&CAN2_RX_buffer.DATA,8);
                if((CAN2_RX_R_data[0]==0x4B)&&(CAN2_RX_R_data[2]==0x21)&&(CAN2_RX_R_data[3]==0x02))
                {
                    Data_Move2.data_Byte[0]= CAN2_RX_R_data[4];
                    Data_Move2.data_Byte[1]= CAN2_RX_R_data[5];
                   Motion_Data.Electric_Current[Motion_Right] = Data_Move2.data_int16;
                }
                
            }else if(CAN2_RX_buffer.RXHeader.StdId == 0x581)
            {
                memcpy(CAN2_RX_L_data,&CAN2_RX_buffer.DATA,8);
                if((CAN2_RX_L_data[0]==0x4B)&&(CAN2_RX_L_data[2]==0x21)&&(CAN2_RX_L_data[3]==0x01))
                {
                    Data_Move2.data_Byte[0]= CAN2_RX_L_data[4];
                    Data_Move2.data_Byte[1]= CAN2_RX_L_data[5];
                    Motion_Data.Electric_Current[Motion_Left] =Data_Move2.data_int16;
                }
            }
        }
}
/* USER CODE END CAN */

/* USER CODE RECEIVE USART1 */
void Recive_Handle_Usart1 (void)
{
//    LOGI("ID_DATA:%d,%d\n",RS232_rxbuffer.recv_end_flag,RS232_rxbuffer.rx_len);
    if(Rxbuffer[Usart_1].recv_end_flag == 1)  //接收完成标志 
    {
        Rxbuffer[Usart_1].recv_end_flag = 0;//清除接收结束标志位
        if(Rxbuffer[Usart_1].rx_len<25)
        {
            memset(Rxbuffer[Usart_1].RX_BUFFER,0,BUFFER_SIZE);//清除DMA缓存区
        }else
        {
            for(uint16_t i=0;i<Rxbuffer[Usart_1].rx_len;i++)
            {
                U_RX_BUFFER[Usart_1].RX_BUFFER[U_RX_BUFFER[Usart_1].Write] = Rxbuffer[Usart_1].RX_BUFFER[i];
                U_RX_BUFFER[Usart_1].Write = (U_RX_BUFFER[Usart_1].Write+1)& RXBUFFLEN;	
            }
        }
        memset(Rxbuffer[Usart_1].RX_BUFFER,0,BUFFER_SIZE);//清除DMA缓存区
        Rxbuffer[Usart_1].rx_len = 0;//清除接收数量
  }
    HAL_UART_Receive_DMA(&huart1,Rxbuffer[Usart_1].RX_BUFFER,BUFFER_SIZE);//重新打开DMA接收
    sbus_loopdead (); //SBUS 数据处理
}
/* USER CODE END USART1 */

/* USER CODE RECEIVE USART3 */
void Recive_Handle_Usart3 (void)
{
    if(Rxbuffer[Usart_3].recv_end_flag == 1)  //接收完成标志 
    {
        Rxbuffer[Usart_3].recv_end_flag = 0;//清除接收结束标志位
        memset(U_RX_BUFFER[Usart_3].RX_BUFFER,0,BUFFER_SIZE);//清除buf缓存区
        memcpy(U_RX_BUFFER[Usart_3].RX_BUFFER,Rxbuffer[Usart_3].RX_BUFFER,Rxbuffer[Usart_3].rx_len);
        memset(Rxbuffer[Usart_3].RX_BUFFER,0,BUFFER_SIZE);//清除DMA缓存区
        Rxbuffer[Usart_3].rx_len = 0;//清除接收数量
  }
     HAL_UART_Receive_DMA(&huart3,Rxbuffer[Usart_3].RX_BUFFER,BUFFER_SIZE);//重新打开DMA接收
}
/* USER CODE END USART3 */

/* USER CODE END USART6 */

/* USER CODE HANDLE SBUS */
static void update_sbus(uint8_t *buf)
{
        SBUS_CH.CH1 = ((int16_t)buf[ 1] >> 0 | ((int16_t)buf[ 2] << 8 )) & 0x07FF;
        SBUS_CH.CH2 = ((int16_t)buf[ 2] >> 3 | ((int16_t)buf[ 3] << 5 )) & 0x07FF;
        SBUS_CH.CH3 = ((int16_t)buf[ 3] >> 6 | ((int16_t)buf[ 4] << 2 ) | (int16_t)buf[ 5] << 10 ) & 0x07FF;
        SBUS_CH.CH4 = ((int16_t)buf[ 5] >> 1 | ((int16_t)buf[ 6] << 7 )) & 0x07FF;
        SBUS_CH.CH5 = ((int16_t)buf[ 6] >> 4 | ((int16_t)buf[ 7] << 4 )) & 0x07FF;
        SBUS_CH.CH6 = ((int16_t)buf[ 7] >> 7 | ((int16_t)buf[ 8] << 1 ) | (int16_t)buf[9] << 9 ) & 0x07FF;
        SBUS_CH.CH7 = ((int16_t)buf[ 9] >> 2 | ((int16_t)buf[10] << 6 )) & 0x07FF;
        SBUS_CH.CH8 = ((int16_t)buf[10] >> 5 | ((int16_t)buf[11] << 3 )) & 0x07FF;
        SBUS_CH.CH9 = ((int16_t)buf[12] << 0 | ((int16_t)buf[13] << 8 )) & 0x07FF;
        SBUS_CH.CH10 = ((int16_t)buf[13] >> 3 | ((int16_t)buf[14] << 5 )) & 0x07FF;
        SBUS_CH.CH11 = ((int16_t)buf[14] >> 6 | ((int16_t)buf[15] << 2 ) | (int16_t)buf[16] << 10 ) & 0x07FF;
        SBUS_CH.CH12 = ((int16_t)buf[16] >> 1 | ((int16_t)buf[17] << 7 )) & 0x07FF;
        SBUS_CH.CH13 = ((int16_t)buf[17] >> 4 | ((int16_t)buf[18] << 4 )) & 0x07FF;
        SBUS_CH.CH14 = ((int16_t)buf[18] >> 7 | ((int16_t)buf[19] << 1 ) | (int16_t)buf[20] << 9 ) & 0x07FF;
        SBUS_CH.CH15 = ((int16_t)buf[20] >> 2 | ((int16_t)buf[21] << 6 )) & 0x07FF;
        SBUS_CH.CH16 = ((int16_t)buf[21] >> 5 | ((int16_t)buf[22] << 3 )) & 0x07FF;
}
static void Remote_re_con(void)
{
        
        
    //遥控器 E
    if(SBUS_CH.CH5==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_0=1;
         Re_Bool.Bit.bool_1=0;
    }else if(SBUS_CH.CH5==SBUS_KEY_MIDDLE)
        {Re_Bool.Bit.bool_0=0;
         Re_Bool.Bit.bool_1=0;
    }else if(SBUS_CH.CH5==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_0=0;
         Re_Bool.Bit.bool_1=1;
    }else{Re_Bool.Bit.bool_0=0;
          Re_Bool.Bit.bool_1=0;}
    //遥控器 F
    if(SBUS_CH.CH6==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_2=1;
         Re_Bool.Bit.bool_3=0;
    }else if(SBUS_CH.CH6==SBUS_KEY_MIDDLE)
        {Re_Bool.Bit.bool_2=0;
         Re_Bool.Bit.bool_3=0;
    }else if(SBUS_CH.CH6==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_2=0;
         Re_Bool.Bit.bool_3=1;
    }else{Re_Bool.Bit.bool_2=0;
          Re_Bool.Bit.bool_3=0;}
    //遥控器 A
    if(SBUS_CH.CH7==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_4=0;}else if(SBUS_CH.CH7==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_4=1;}else{Re_Bool.Bit.bool_4=0;}
    //遥控器 B
    if(SBUS_CH.CH8==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_5=0;}else if(SBUS_CH.CH8==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_5=1;}else{Re_Bool.Bit.bool_5=0;}
    //遥控器 C
    if(SBUS_CH.CH9==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_6=0;}else if(SBUS_CH.CH9==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_6=1;}else{Re_Bool.Bit.bool_6=0;}
    //遥控器 D
    if(SBUS_CH.CH10==SBUS_KEY_MIN)
        {Re_Bool.Bit.bool_7=0;}else if(SBUS_CH.CH10==SBUS_KEY_MAX)
        {Re_Bool.Bit.bool_7=1;}else{Re_Bool.Bit.bool_7=0;}
        
        
        
}

void sbus_loopdead (void)
{
    uint8_t i; 
    if( U_RX_BUFFER[Usart_1].Write == U_RX_BUFFER[Usart_1].Read)return; 	//没有新数据
    if( (U_RX_BUFFER[Usart_1].RX_BUFFER[U_RX_BUFFER[Usart_1].Read]==0x0f) && \
        (U_RX_BUFFER[Usart_1].RX_BUFFER[(U_RX_BUFFER[Usart_1].Read +24)& RXBUFFLEN] == 0x00 )&&\
        (U_RX_BUFFER[Usart_1].RX_BUFFER[(U_RX_BUFFER[Usart_1].Read +23)& RXBUFFLEN] == 0x00)  ) //此处为SBUS协议  根据情况修改
        {
            if(((U_RX_BUFFER[Usart_1].Write - U_RX_BUFFER[Usart_1].Read)& RXBUFFLEN)>=SBUS_SINGLE_BUFSIZE) 
            {
                for(i=0;i<SBUS_SINGLE_BUFSIZE;i++)
                {
                    SBUS_DATA[i] = U_RX_BUFFER[Usart_1].RX_BUFFER[(U_RX_BUFFER[Usart_1].Read +i)&RXBUFFLEN];  //将数据装入数组
                }
                update_sbus(SBUS_DATA); //将sbus信号转化为通道值
                Remote_re_con();
                U_RX_BUFFER[Usart_1].Read = (U_RX_BUFFER[Usart_1].Read +SBUS_SINGLE_BUFSIZE)&RXBUFFLEN;
                U_RX_BUFFER[Usart_1].Flag=0;
            }
//            LOGG("SBUS_DATA   %d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d",SBUS_CH.CH1,SBUS_CH.CH2,SBUS_CH.CH3,SBUS_CH.CH4,SBUS_CH.CH5,SBUS_CH.CH6 \
//                   ,SBUS_CH.CH7,SBUS_CH.CH8,SBUS_CH.CH9,SBUS_CH.CH10,SBUS_CH.CH11,SBUS_CH.CH12,SBUS_CH.CH13  \
//                    ,SBUS_CH.CH14,SBUS_CH.CH15,SBUS_CH.CH13);
//            LOGG("\n");
        }
        else 
        {
        U_RX_BUFFER[Usart_1].Read = (U_RX_BUFFER[Usart_1].Read +1)&RXBUFFLEN;
        }	
}

/* USER CODE END SBUS */
/**遥控器接收函数**/
void Sbus_Can_Handle(void)
{
    /*
	上电检测各个摇杆要在中间位置才可以进行操作
	自检依靠标志为检测
*/
    //(SBUS_CH.CH10 == SBUS_KEY_MIN)&&\
      //(SBUS_CH.CH9 == SBUS_KEY_MIN)&&\
    
    if(
        (SBUS_CH.CH8 == SBUS_KEY_MIN)&&\
        (SBUS_CH.CH7 == SBUS_KEY_MIN)&&\
        (SBUS_CH.CH6 == SBUS_KEY_MIDDLE)&&\
        (SBUS_CH.CH5 == SBUS_KEY_MIDDLE)&&\
        (!sbus_init_flag)&&\
        (SBUS_ROCK_DOWNDEAD<SBUS_CH.CH2)&&\
        (SBUS_CH.CH2< SBUS_ROCK_UPDEAD)&&\
        (SBUS_ROCK_LEFTDEAD<SBUS_CH.CH1)&&\
        (	SBUS_CH.CH1< SBUS_ROCK_RIGHTDEAD))
    {
        sbus_init_flag = 1;
    }
//LOGG("%d\n",sbus_init_flag);
    if(sbus_init_flag)
    {
        Remote_re_con();//按键
        if(Re_Bool.Bit.bool_5)//手动运行
        { 
            if((!Re_Bool.Bit.bool_2)&&(!Re_Bool.Bit.bool_3))
            {
                Max_Speed = 800;
            }else if((Re_Bool.Bit.bool_2)&&(!Re_Bool.Bit.bool_3))
            {
                Max_Speed = 500;
            }else if((!Re_Bool.Bit.bool_2)&&(Re_Bool.Bit.bool_3))
            {
                Max_Speed = 1000;
            }else Max_Speed = 800;
            Sbus_Motion_Re();//运动
            Can_Drive_Send(Speed_left_flag,Speed_left,Speed_right_flag,Speed_right);//发送
           // KY_CAN_Send(Speed_left_flag,Speed_left,Speed_right_flag,Speed_right);
            //  PWM_send();//同时启停
            PWM_Send_Tim();// 启动带先后响应
            memset(&SBUS_CH,0,sizeof(SBUS_CH) );//  失控保护清零
        }else
        {
            Can_Drive_Stop();
           // KY_Can_Stop();
        }
        
        
    }
    
}

//void PWM_send(void)
//{
//    if(Re_Bool.Bit.bool_0)
//    {
//        if((!Re_Bool.Bit.bool_1)&&(!Re_Bool.Bit.bool_2))
//        {
//            
//                if(PWM_FLAG !=1)
//                {
//                    Tim_PWM(&htim9,1,NULL,70);//搅拌
//                    Tim_PWM(&htim9,2,NULL,28);//抛洒
//                    PWM_FLAG=1;
//                   
//                }
//        }else if((Re_Bool.Bit.bool_1)&&(!Re_Bool.Bit.bool_2))
//            {
//                
//            if(PWM_FLAG!=2)
//            {
//                Tim_PWM(&htim9,1,NULL,75);
//                Tim_PWM(&htim9,2,NULL,56);
//                PWM_FLAG=2;
//               
//            }
//            
//        }else if((!Re_Bool.Bit.bool_1)&&(Re_Bool.Bit.bool_2))
//        {
//            if(PWM_FLAG!=3)
//            {
//                Tim_PWM(&htim9,1,NULL,80);
//                Tim_PWM(&htim9,2,NULL,84);
//                PWM_FLAG=3;
//            }
//            
//        }
//    }else
//    {
//        if(PWM_FLAG!=4)
//        {
//            Tim_PWM(&htim9,1,NULL,0);
//            Tim_PWM(&htim9,2,NULL,0);
//            PWM_FLAG=4;
//            
//        }
//        
//    }
//    
//}
//搅拌抛洒 先后 启动
void PWM_Send_Tim(void)
{
    if (!Re_Bool.Bit.bool_4) {
        // 失能时重置所有状态
        if (pwm_state != PWM_STATE_DISABLED) {
            Tim_PWM(&htim4, 3, NULL, 0);  // 搅拌PWM关闭  
            Tim_PWM(&htim4, 4, NULL, 0);  // 抛洒PWM关闭
            PWM_FLAG = 0;
            pwm_state = PWM_STATE_DISABLED;
            startup_delay_tick = 0;
        }
        return;
    }

  //  PD14     ------> TIM4_CH3
  //  PD15     ------> TIM4_CH4
    if (pwm_state == PWM_STATE_DISABLED) {
        // 执行最低档抛洒（仅抛洒，搅拌暂不启动）
        Tim_PWM(&htim4, 3, NULL, 0);      // 搅拌关闭
        Tim_PWM(&htim4, 4, NULL, 35);     // 抛洒PWM  可改
        pwm_state = PWM_STATE_STARTUP;
        // 记录启动时间  FreeRTOS   滴答
        startup_delay_tick = xTaskGetTickCount();
        return;
    }

    if (pwm_state == PWM_STATE_STARTUP) {
        // 是否达到启动延迟时间  5s
        if (xTaskGetTickCount() - startup_delay_tick < pdMS_TO_TICKS(STARTUP_DELAY_MS)) {
            return;
        } else {
            // 进入正常状态
            pwm_state = PWM_STATE_NORMAL;
        }
    }

    // 档位切换（只执行一次）
    if (pwm_state == PWM_STATE_NORMAL) {
        // 中档位
        if ((!Re_Bool.Bit.bool_0) && (!Re_Bool.Bit.bool_1) && (PWM_FLAG != 1)) {
            Tim_PWM(&htim4, 3, NULL, 90);  // 搅拌
            Tim_PWM(&htim4, 4, NULL, 56);  // 抛洒
            PWM_FLAG = 1;
        }
        // 低档位
        else if ((Re_Bool.Bit.bool_0) && (!Re_Bool.Bit.bool_1) && (PWM_FLAG != 2)) {
            Tim_PWM(&htim4, 3, NULL, 85);
            Tim_PWM(&htim4, 4, NULL, 28);
            PWM_FLAG = 2;
        }
        // 高档位
        else if ((!Re_Bool.Bit.bool_0) && (Re_Bool.Bit.bool_1) && (PWM_FLAG != 3)) {
            Tim_PWM(&htim4, 3, NULL, 95);
            Tim_PWM(&htim4, 4, NULL, 84);
            PWM_FLAG = 3;
        }
    }
}

//分时发送请求数据
void Time_Slicing(void)
{
    //uint8_t data[8] = {0x40,0x00,0x21,0x01,0x00,0x00,0x00,0x00}; //请求错误状态
    time_sharing++;
   if(time_sharing%5==0) //250MS询问一次
   {
       uint8_t data[8] = {0x40,0x00,0x21,0x01,0x00,0x00,0x00,0x00};//请求电流
       can_send_msg(&hcan2,0,KY_Driver, data,8 );
   }
   if(time_sharing%5==4) //250MS询问一次
   {
       uint8_t data[8] = {0x40,0x00,0x21,0x02,0x00,0x00,0x00,0x00};//请求电流
       can_send_msg(&hcan2,0,KY_Driver, data,8 );
   }
    if(time_sharing>50000)time_sharing=0;
}