#include "ti_msp_dl_config.h"
#include <stdint.h>
#include <stdbool.h>
#include "wp_math.h"
#include "datatype.h"
#include "systick.h"
#include "bsp_usart.h"
#include "bsp_led.h"
#include "sensor.h"
#include "user_ctrl.h"
#include "subtask_demo.h"
#include "developer_mode.h"
#include "nclink.h"



uint8_t NCLink_Head[2]={0xFF,0xFC};//数据帧头
uint8_t NCLink_End[2] ={0xA1,0xA2};//数据帧尾
uint8_t nclink_databuf[128];//待发送数据缓冲区
float param_value[RESERVED_PARAM_NUM]={25};
third_party_state current_state;

/***************************************************************************************
@函数名：Serial_Data_Send(uint8_t *buf, uint32_t cnt)
@入口参数：buf:待发送数据
			     cnt:待发送字长
@出口参数：无
功能描述：串口发送函数
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void Serial_Data_Send(uint8_t *buf, uint32_t cnt)  
{
	usart1_send_bytes(buf,cnt);
}

/***************************************************************************************
@函数名：Pilot_Status_Tick(void) 
@入口参数：无
@出口参数：无
功能描述：飞控接收状态显示
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void Pilot_Status_Tick(void)  
{
	Bling_Set(&Light_Red, 500, 50, 0.5, 0, BLING_R_PORT, GPIO_RGB_RED_PIN, 0);//用户移植时，重写此函数
}


union
{
	unsigned char floatByte[4];
	float floatValue;
}FloatUnion;


/***************************************************************************************
@函数名：void Float2Byte(float *FloatValue, unsigned char *Byte, unsigned char Subscript)
@入口参数：FloatValue:float值
			     Byte:数组
		       Subscript:指定从数组第几个元素开始写入
@出口参数：无
功能描述：将float数据转成4字节数据并存入指定地址
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void Float2Byte(float *FloatValue, unsigned char *Byte, unsigned char Subscript)
{
	FloatUnion.floatValue = (float)2;
	if(FloatUnion.floatByte[0] == 0)//小端模式
	{
		FloatUnion.floatValue = *FloatValue;
		Byte[Subscript]     = FloatUnion.floatByte[0];
		Byte[Subscript + 1] = FloatUnion.floatByte[1];
		Byte[Subscript + 2] = FloatUnion.floatByte[2];
		Byte[Subscript + 3] = FloatUnion.floatByte[3];
	}
	else//大端模式
	{
		FloatUnion.floatValue = *FloatValue;
		Byte[Subscript]     = FloatUnion.floatByte[3];
		Byte[Subscript + 1] = FloatUnion.floatByte[2];
		Byte[Subscript + 2] = FloatUnion.floatByte[1];
		Byte[Subscript + 3] = FloatUnion.floatByte[0];
	}
}


/***************************************************************************************
@函数名：void Byte2Float(unsigned char *Byte, unsigned char Subscript, float *FloatValue)
@入口参数：Byte:数组
			     Subscript:指定从数组第几个元素开始写入
		       FloatValue:float值
@出口参数：无
功能描述：从指定地址将4字节数据转成float数据
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void Byte2Float(unsigned char *Byte, unsigned char Subscript, float *FloatValue)
{
	FloatUnion.floatByte[0]=Byte[Subscript];
	FloatUnion.floatByte[1]=Byte[Subscript + 1];
	FloatUnion.floatByte[2]=Byte[Subscript + 2];
	FloatUnion.floatByte[3]=Byte[Subscript + 3];
	*FloatValue=FloatUnion.floatValue;
}


/***************************************************************************************
@函数名：NCLink_Send_Userdata(float userdata1	 ,float userdata2,
											        float userdata3  ,float userdata4,
											        float userdata5  ,float userdata6)
@入口参数：userdata1:用户数据1
					 userdata2:用户数据2
					 userdata3:用户数据3
					 userdata4:用户数据4
					 userdata5:用户数据5
					 userdata6:用户数据6
@出口参数：无
功能描述：发送用户数据给地面站
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void NCLink_Send_Userdata(float userdata1	 ,float userdata2,
											    float userdata3  ,float userdata4,
											    float userdata5  ,float userdata6)
{
  uint8_t sum=0,_cnt=0,i=0;
  nclink_databuf[_cnt++]=NCLink_Head[0];
  nclink_databuf[_cnt++]=NCLink_Head[1];
  nclink_databuf[_cnt++]=NCLINK_USER;
  nclink_databuf[_cnt++]=0;
		
	Float2Byte(&userdata1,nclink_databuf,_cnt);//4
	_cnt+=4;
	Float2Byte(&userdata2,nclink_databuf,_cnt);//8
	_cnt+=4;
	Float2Byte(&userdata3,nclink_databuf,_cnt);//12
	_cnt+=4;
	Float2Byte(&userdata4,nclink_databuf,_cnt);//16
	_cnt+=4;
	Float2Byte(&userdata5,nclink_databuf,_cnt);//20
	_cnt+=4;
	Float2Byte(&userdata6,nclink_databuf,_cnt);//24
	_cnt+=4;//28

  nclink_databuf[3] = _cnt-4;
	
  for(i=0;i<_cnt;i++) sum ^= nclink_databuf[i]; 
  nclink_databuf[_cnt++]=sum;
	
	nclink_databuf[_cnt++]=NCLink_End[0];
	nclink_databuf[_cnt++]=NCLink_End[1];
  Serial_Data_Send(nclink_databuf,_cnt);
}


/***************************************************************************************
@函数名：NCLink_Send_CalRawdata2(float mag_x_raw ,float mag_y_raw ,float mag_z_raw)
@入口参数：mag_x_raw: 磁力计校准X轴原始数据
					 mag_y_raw: 磁力计校准Y轴原始数据
					 mag_z_raw: 磁力计校准Z轴原始数据
@出口参数：无
功能描述：发送传感器校准原始数据给地面站
@作者：无名小哥
@日期：2020年01月17日
****************************************************************************************/
void NCLink_Send_CalRawdata2(float mag_x_raw ,float mag_y_raw ,float mag_z_raw)
{
  uint8_t sum=0,_cnt=0,i=0;
  nclink_databuf[_cnt++]=NCLink_Head[0];
  nclink_databuf[_cnt++]=NCLink_Head[1];
  nclink_databuf[_cnt++]=NCLINK_SEND_CAL_RAW2;
  nclink_databuf[_cnt++]=0;
	
	Float2Byte(&mag_x_raw,nclink_databuf,_cnt);
	_cnt+=4;
	Float2Byte(&mag_y_raw,nclink_databuf,_cnt);
	_cnt+=4;
	Float2Byte(&mag_z_raw,nclink_databuf,_cnt);
	_cnt+=4;	
	
  nclink_databuf[3] = _cnt-4;	
  for(i=0;i<_cnt;i++) sum ^= nclink_databuf[i]; 
  nclink_databuf[_cnt++]=sum;
	
	nclink_databuf[_cnt++]=NCLink_End[0];
	nclink_databuf[_cnt++]=NCLink_End[1];
	Serial_Data_Send(nclink_databuf,_cnt);
}



void pilot_msg_notify(void)
{		
	static uint8_t cnt=0;cnt++;
	if(cnt>=20)
	{
		Bling_Set(&Light_Red, 500, 50, 0.5, 0, BLING_R_PORT, GPIO_RGB_RED_PIN, 0);//用户移植时，重写此函数
		cnt=0;
	}
}

/************************************************************************************/
systime fb_t[2];
float fb_dt=0;
uint32_t miss_cnt[2]={0};
float miss_rate=0;
void NCLink_Data_Prase_Pilot_Status(uint8_t data)
{
  static uint8_t fb_buf[100];
  static uint8_t data_len = 0,data_cnt = 0;
  static uint8_t state = 0;
  if(state==0&&data==NCLink_Head[0])//判断帧头1
  {
		get_systime(&fb_t[0]);
    state=1;
    fb_buf[0]=data;
		miss_cnt[1]++;
  }
  else if(state==1&&data==NCLink_Head[1])//判断帧头2
  {
    state=2;
    fb_buf[1]=data;
		miss_cnt[1]++;
  }
  else if(state==2&&data<0XF1)//功能字节
  {
    state=3;
    fb_buf[2]=data;
		miss_cnt[1]++;
  }
  else if(state==3&&data<100)//有效数据长度
  {
    state = 4;
    fb_buf[3]=data;
    data_len = data;
    data_cnt = 0;
		miss_cnt[1]++;
  }
  else if(state==4&&data_len>0)//数据接收
  {
    data_len--;
    fb_buf[4+data_cnt++]=data;
    if(data_len==0)  state = 5;
		miss_cnt[1]++;
  }
  else if(state==5)//异或校验
  {
    state = 6;
    fb_buf[4+data_cnt++]=data;
		miss_cnt[1]++;
  }
	else if(state==6&&data==NCLink_End[0])//帧尾0
	{
		state = 7;
		fb_buf[4+data_cnt++]=data;
		miss_cnt[1]++;
	}
	else if(state==7&&data==NCLink_End[1])//帧尾1
	{
			state = 0;
			fb_buf[4+data_cnt]=data;
			uint16_t _cnt=data_cnt+5;
			uint8_t sum = 0;
			for(uint8_t i=0;i<(_cnt-3);i++)  sum ^= *(fb_buf+i);
			if(!(sum==*(fb_buf+_cnt-3)))    																		    return;//判断sum	
			if(!(*(fb_buf)==NCLink_Head[0]&&*(fb_buf+1)==NCLink_Head[1]))           return;//判断帧头
			if(!(*(fb_buf+_cnt-2)==NCLink_End[0]&&*(fb_buf+_cnt-1)==NCLink_End[1])) return;//帧尾校验 		
			if(*(fb_buf+2)==NCLINK_SEND_AHRS)//数据帧类型判断
			{
				Byte2Float(fb_buf, 4,&uav_ins.position[_EAST]);
				Byte2Float(fb_buf, 8,&uav_ins.position[_NORTH]);
				Byte2Float(fb_buf, 12,&uav_ins.position[_UP]);
				Byte2Float(fb_buf, 16,&uav_ins.speed[_EAST]);
				Byte2Float(fb_buf, 20,&uav_ins.speed[_NORTH]);
				Byte2Float(fb_buf, 24,&uav_ins.speed[_UP]);
				Byte2Float(fb_buf, 28,&uav_ins.accel_cmpss[_EAST]);
				Byte2Float(fb_buf, 32,&uav_ins.accel_cmpss[_NORTH]);
				Byte2Float(fb_buf, 36,&uav_ins.accel_cmpss[_UP]);
				Byte2Float(fb_buf, 40,&uav_ahrs.rpy_gyro_dps[0]);
				Byte2Float(fb_buf, 44,&uav_ahrs.rpy_gyro_dps[1]);
				Byte2Float(fb_buf, 48,&uav_ahrs.rpy_gyro_dps[2]);
				uav_ahrs.quaternion[0]=0.0001f*( (int16_t)(*(fb_buf+52)<<8)|*(fb_buf+53) );
				uav_ahrs.quaternion[1]=0.0001f*( (int16_t)(*(fb_buf+54)<<8)|*(fb_buf+55) );
				uav_ahrs.quaternion[2]=0.0001f*( (int16_t)(*(fb_buf+56)<<8)|*(fb_buf+57) );
				uav_ahrs.quaternion[3]=0.0001f*( (int16_t)(*(fb_buf+58)<<8)|*(fb_buf+59) );

				uav_ahrs.quaternion_init_ok  =*(fb_buf+60);
				current_state.rec_update_flag=*(fb_buf+61);
				offboard_start_flag=*(fb_buf+62);

				Flight.yaw_ctrl_end=*(fb_buf+63);	
				Flight.yaw_ctrl_response=*(fb_buf+64);
			}
			get_systime(&fb_t[1]);
			fb_dt=fb_t[1].current_time-fb_t[0].current_time;
			pilot_msg_notify();
			
			miss_cnt[1]++;
		  miss_cnt[0]++;
		  miss_rate=(float)(miss_cnt[0]*68)/miss_cnt[1];
	}
  else 
	{
		state = 0;
		miss_cnt[1]++;
	}
}