/*
 * msp_DH_W_H.c
 *
 *  Created on: 2023年12月26日
 *      Author: Administrator
 */


#include "MSP/msp_DH_W_H.h"
#include "MSP/msp_TL720D.h"
#include "MSP/msp_LeiSai_Motor.h"
#include "MSP/msp_Modbus_crc16.h"
UART_HandleTypeDef* Huart_DH_WH;
uint8_t DH_stas[30];
uint8_t DH_stas2[8];
SP_MSP_DH_Handset_Parameters *DH_WH_BING;
int32_t DH_Falg=1;
uint8_t Screen_buf[17];
extern SP_MSP_RF_TL720D_Parameters *RF_720D;
extern SP_MSP_LeiSai_MotorParameters* M_LeiSai;
extern uint8_t writeSpeed;
extern int auto_flag;
extern float height1;
extern float height2;
extern float dis_to_ground;
extern int index_sign_screen;
int DH_Screen_Flag=0;
int16_t mid_speed=0;
int16_t mid_angle=0;
int watch333=0;

void  GF_DH_Hangset_Unscramble_Callback();
void DH_Command_Comp(uint8_t *buf);
void DH_Command_Comp_Sub(uint8_t *buf_Sub);


const int16_t SpeedValue[11]={0,2,5,10,15,20,22,24,26,30,30}; //m/min
//从C#中拷贝过来的每个档位对应的速度大小（此处为初始值，后续可以根据手机APP来改变此值）
//轮子直径为0.942m，传动比70，500/3为(P     X=500*V/3)(V为电机的转速RPM/min)
int32_t speedvalue_1 = SpeedValue[1]/0.942*70*500/3; 	//2/0.942*70  148
int32_t speedvalue_2 = SpeedValue[2]/0.942*70*500/3 ;	//371
int32_t speedvalue_3 = SpeedValue[3]/0.942*70*500/3 ;	//743
int32_t speedvalue_4 = SpeedValue[4]/0.942*70*500/3 ;	//1114
int32_t speedvalue_5 = SpeedValue[5]/0.942*70*500/3 ;	//1486
int32_t speedvalue_6 = SpeedValue[6]/0.942*70*500/3 ;	//1634
int32_t speedvalue_7 = SpeedValue[7]/0.942*70*500/3 ;	//1783
int32_t speedvalue_8 = SpeedValue[8]/0.942*70*500/3 ;	//1932
int32_t speedvalue_9 = SpeedValue[9]/0.942*70*500/3 ;	//2080
int32_t speedvalue_10 = SpeedValue[10]/0.942*70*500/3 ;	//2229

uint8_t GF_MSP_DH_Wash_H_Init(UART_HandleTypeDef* huart,SP_MSP_DH_Handset_Parameters *_DH_Handset_W_H)
{
	Huart_DH_WH=huart;
	GF_BSP_Interrupt_Add_CallBack(DF_BSP_InterCall_RS485_3_RxCpltCallback, GF_DH_Hangset_Unscramble_Callback);//绑定中断回调函数
	HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
	DH_WH_BING = _DH_Handset_W_H;
	return 1;
}


void GF_MSP_DH_Stu_Inquiry()    //遥控器请求指令
{
	uint8_t Inquiry_Order[8]={0X03, 0X04, 0X00, 0X00, 0X00, 0X0A, 0X71, 0XEF};
	GF_BSP_UART_Transmit(3, Inquiry_Order, 8);//这句话的关联有问题

}



void  GF_DH_Hangset_Unscramble_Callback()
{
	switch(DH_Falg)
	{
		case 1:
			if(DH_stas[0] == 0x03)
			{
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
				DH_Falg = 2;
			}
			else {
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
				DH_Falg = 1;
			}
		break;
		case 2:
			if(DH_stas[0] == 0x04)
			{
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
				DH_Falg = 3;
			}
			else {
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
				DH_Falg = 1;
			}
		break;
		case 3:
			if(DH_stas[0] == 0x14)
			{
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,22);
				DH_Falg = 4;
			}
			else {
				HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
				DH_Falg = 1;
			}
		break;
		case 4:
			DH_Command_Comp(DH_stas);
			HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,1);
			DH_Falg = 1;
		break;

	}
//
//	if(DH_stas[0]==0x03&&DH_stas[1]==0x04&&DH_stas[2]==0x0C)
//	{
//		DH_Command_Comp(DH_stas);
//		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,17);
//	}
//	else {
//		HAL_UART_Receive_IT(Huart_DH_WH,DH_stas,17);
//	}

}

int count1=0;

void GF_MSP_DH_Stu_Screen()//写屏幕
{
	if(count1<100)
	{
		count1++;
	}
	else
	{
		count1=0;
	}

	unsigned short CRC_Value;


	Screen_buf[0]=0x03;
	Screen_buf[1]=0x10;
	Screen_buf[2]=0x00;
	Screen_buf[3]=0x00;
	Screen_buf[4]=0x00;
	Screen_buf[5]=0x04;
	Screen_buf[6]=0x08;

	//Screen_buf[7]=(int16_t)height1>>8;
	Screen_buf[7]=((DH_WH_BING->Left_compensation * 9 + 127) / 255 * 100) >> 8;  //改为环道距离十位
	Screen_buf[8]=(DH_WH_BING->Left_compensation * 9 + 127) / 255 * 100;
	Screen_buf[9]=((DH_WH_BING->Right_compensation * 9 + 127) / 255 * 100) >> 8;  //改为换道距离个位
	Screen_buf[10]=(DH_WH_BING->Right_compensation * 9 + 127) / 255 * 100;
	mid_angle=(RF_720D->RF_Angle_Yaw );
	if(mid_angle<0)
	{
		mid_angle=-mid_angle;
	}

	Screen_buf[11]=mid_angle>>8;;
	Screen_buf[12]=mid_angle;
	Screen_buf[13]=DH_WH_BING->motor_speed_level >> 8;
	Screen_buf[14]=DH_WH_BING->motor_speed_level;


	CRC_Value=ModBusCRC16(Screen_buf,15); // CRC校验
	Screen_buf[15]= (uint8_t)CRC_Value;
	Screen_buf[16]= (uint8_t)( CRC_Value>>8);

	GF_BSP_UART_Transmit(3, Screen_buf, 17);//这句话的关联有问题

}

void DH_Command_Comp(uint8_t *buf)
{
	//心跳
	DH_WH_BING->Heart_Beat = buf[19];

	//左右补偿
	DH_WH_BING->Left_compensation = buf[5];
	DH_WH_BING->Right_compensation = buf[7];

   //前后左右位
   if(buf[9]&0x04)    //前进被置位
   {
	   DH_WH_BING->for_flag=1;
	   DH_WH_BING->back_flag=0;
	   DH_WH_BING->left_flag=0;
	   DH_WH_BING->right_flag=0;

   }
   else if(buf[9]&0x08)   //后退被置位
   {
	   DH_WH_BING->back_flag=1;
	   DH_WH_BING->for_flag=0;
	   DH_WH_BING->left_flag=0;
	   DH_WH_BING->right_flag=0;
   }
   else if(buf[9]&0x10)   //左转被置位
      {
   	   DH_WH_BING->back_flag=0;
   	   DH_WH_BING->for_flag=0;
   	   DH_WH_BING->left_flag=1;
   	   DH_WH_BING->right_flag=0;
      }
   else if(buf[9]&0x20)   //右转被置位
   {
       DH_WH_BING->back_flag=0;
   	   DH_WH_BING->for_flag=0;
   	   DH_WH_BING->left_flag=0;
   	   DH_WH_BING->right_flag=1;
   }
   else if((buf[9]&0x3c)==0)   //停止被置位
   {
       DH_WH_BING->back_flag=0;
   	   DH_WH_BING->for_flag=0;
   	   DH_WH_BING->left_flag=0;
   	   DH_WH_BING->right_flag=0;
   }


   //速度档位
   if(buf[9]&0x40)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[0];
	   DH_WH_BING->motor_speed_value=0;
   }
   else if(buf[9]&0x80)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[1];
	   DH_WH_BING->motor_speed_value=speedvalue_2;
   }
   else if(buf[8]&0x01)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[2];
	   DH_WH_BING->motor_speed_value=speedvalue_3;
   }
   else if(buf[8]&0x02)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[3];
	   DH_WH_BING->motor_speed_value=speedvalue_4;
   }
   else if(buf[8]&0x04)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[4];
	   DH_WH_BING->motor_speed_value=speedvalue_5;
   }
   else if(buf[8]&0x08)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[5];
	   DH_WH_BING->motor_speed_value=speedvalue_6;
   }
   else if(buf[8]&0x10)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[6];
	   DH_WH_BING->motor_speed_value=speedvalue_7;
   }
   else if(buf[8]&0x20)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[7];
	   DH_WH_BING->motor_speed_value=speedvalue_8;
   }
   else if(buf[8]&0x40)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[8];
	   DH_WH_BING->motor_speed_value=speedvalue_9;
   }
   else if(buf[8]&0x80)
   {
	   DH_WH_BING->motor_speed_level=SpeedValue[9];
	   DH_WH_BING->motor_speed_value=speedvalue_10;
   }


   //急停
   if(buf[9]&0x01)
   {
	   DH_WH_BING->EMERGENCY_STOP=1;
   }

   //水平作业/竖直作业   ->改为作业开启和作业关闭，上拨作业开启，中和下作业关闭
   if(buf[11]&0x01)
   {
	   DH_WH_BING->working_start1_stop2 = 1;
//	   DH_WH_BING->Horizontal_working=1;
//		DH_WH_BING->Vertical_working=0;
   }
   else if(buf[11]&0x02)
   {
	   DH_WH_BING->working_start1_stop2 = 2;
//	   DH_WH_BING->Vertical_working=1;
//	   DH_WH_BING->Horizontal_working=0;
   }
   else if((buf[11]&0x03)==0)
   {
	   DH_WH_BING->working_start1_stop2 = 2;
//   	   DH_WH_BING->Vertical_working=0;
//   	   DH_WH_BING->Horizontal_working=0;
   }


   //水平左/右换道
   if(buf[11]&0x04)
   {
	  DH_WH_BING->Horizontal_change_road_left=1;
   }
   else if(buf[11]&0x08)
   {
	  DH_WH_BING->Horizontal_change_road_right=1;
   }
   else if(!(buf[7]&0x0c))    //此处是为了实现拨杆拨回中间位置 在上下拨动才会再次触发换道
   {
	  DH_WH_BING->Horizontal_change_road_left=0;
	  DH_WH_BING->Horizontal_change_road_right=0;
	  DH_WH_BING->Horizontal_change_road_finish_flag=1;
   }



   //竖直上/下换道
   if(buf[11]&0x40)
   {
//	   DH_WH_BING->Vertical_change_road_up=1;
	   DH_WH_BING->Vertical_change_road_up1_down2_close0 = 1;
	   DH_WH_BING->Vertical_change_road_finish_flag=0;
   }
   else if(buf[11]&0x80)
   {
//	   DH_WH_BING->Vertical_change_road_down=1;
	   DH_WH_BING->Vertical_change_road_up1_down2_close0 = 2;
	   DH_WH_BING->Vertical_change_road_finish_flag=0;
   }
   else if(!(buf[7]&0xc0))
   {
//	   DH_WH_BING->Vertical_change_road_up=0;
//	   DH_WH_BING->Vertical_change_road_down=0;
	   DH_WH_BING->Vertical_change_road_up1_down2_close0 = 0;
	   DH_WH_BING->Vertical_change_road_finish_flag=1;
	   DH_WH_BING->Allow_execute_again=0;      //此处是为了实现拨杆拨回中间位置 在上下拨动才会再次触发换道
   }

   //备用1/2    ->更改为手动状态下喷枪测试开启和关闭
   if(buf[11]&0x10)
   {
	   DH_WH_BING->test_start1_stop0 = 1;
//	   DH_WH_BING->reserve1=1;
//	   DH_WH_BING->reserve2=0;
   }
   else if(buf[11]&0x20)
   {
	   DH_WH_BING->test_start1_stop0 = 0;
//	   DH_WH_BING->reserve2=1;
//	   DH_WH_BING->reserve1=0;
   }
   else if((buf[11]&0x30)==0)
   {
	   DH_WH_BING->test_start1_stop0 = 0;
       DH_WH_BING->reserve2=0;
       DH_WH_BING->reserve1=0;
   }


   //喷枪开启/关闭
   if(buf[10]&0x01)
   {
	   DH_WH_BING->Open_Up1_Down2 = 1;
//	   DH_WH_BING->open=1;
//	   DH_WH_BING->close=0;
   }
   else if(buf[10]&0x02)
   {
	   DH_WH_BING->Open_Up1_Down2 = 2;
//	   DH_WH_BING->close=1;
//	   DH_WH_BING->open=0;
   }

   if(buf[10]&0x04)
   {
     DH_WH_BING->side1=1;
   }
   else
   {
	 DH_WH_BING->side1=0;
   }


   //侧面1/2/3/4
   if(buf[10]&0x08)
   {
	 DH_WH_BING->side2=1;
   }
   else
   {
     DH_WH_BING->side2=0;
   }

   if(buf[10]&0x10)
   {
	 DH_WH_BING->side3=1;
   }
   else
   {
     DH_WH_BING->side3=0;
   }

   if(buf[10]&0x20)
   {
	 DH_WH_BING->side4=1;
   }
   else
   {
     DH_WH_BING->side4=0;
   }



   DH_WH_BING->For_Back_Value_X=buf[1];         //摇杆前后线性值
   DH_WH_BING->Left_Right_Value_Y=buf[3];       //摇杆左右线性值
}




