/*
 * msp_MF_Gyroscope.c
 *
 *  Created on: Dec 6, 2024
 *      Author: 13752
 */
#include "bsp_Error_Detect.h"
#include "MSP/msp_UWB_20.h"
#include "BHBF_ROBOT.h"
#include "kalman_filter.h"

UWB_20_struct_define* UWB_20_CM;
struct UARTHandler *Huart_UWB_20;

void decode_uwb(uint8_t *buffer, uint16_t length);
void kalmanFun();

KalmanFilter kf;
float dt = 0.1f; // 采样间隔0.1秒

void uwb_sensor_intialize(struct UARTHandler *Handler)
{
    kalman_init(&kf, dt, 0.0f, 0.0f); // 初始位置(0,0)
	Huart_UWB_20 = Handler;
	Huart_UWB_20->Wait_time = 10; //等待10ms 最低不要低于4；
	Huart_UWB_20->UART_Decode = decode_uwb;
	Huart_UWB_20->dispacherController->Dispacher_Enable=0;//不周期性发送
	GF_BSP_Interrupt_Add_CallBack(
			DF_BSP_InterCall_TIM1_100ms_PeriodElapsedCallback, kalmanFun);
}
//01 03 2E AC DA 00 03 00 07 01 16 01 2C 01 41 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 01 00 DF 00 A2 00 00 E8 DC
//01 03 帧头
//2E数据长度；
//AC DA 标识码；
//00 03 输出测距和定位；
//00 07三基站测距成功；
//01 16标签与A基站距离，单位厘米 0116=278CM
//01 2C标签与B基站距离，单位厘米
//01 41标签与C基站距离，单位厘米
//00 01代表定位成功；
//00 DF X轴坐标，单位厘米
//00 A2 Y轴坐标，单位厘米
//00 00 Z轴坐标，单位厘米
//E8 DC     CRC-16校验。

uint16_t Crc_check = 0;
uint8_t Crc_checkL = 0;
uint8_t Crc_checkH = 0;
uint8_t kalman_timer_counts = 0;

void decode_uwb(uint8_t *buffer, uint16_t length)
{
	if(buffer[0] == 0x01 && buffer[1] == 0x03 && buffer[2] == 0x2E && buffer[3] == 0xAC &&
			buffer[4] == 0xDA && buffer[5] == 0x00 && buffer[6] == 0x03)
	{
		Crc_check =  MB_CRC16(buffer,49);
		Crc_checkH = Crc_check >> 8;
		Crc_checkL = Crc_check;
		if((Crc_checkH == buffer[50])&&(Crc_checkL == buffer[49]))
		{
			UWB_20_CM->X_cm=(int16_t)(buffer[43] << 8) | buffer[44];
			UWB_20_CM->Y_cm=(int16_t)(buffer[45] << 8) | buffer[46];
		}
	}
}

float filtered_x, filtered_y;
void kalmanFun()
{
	kalman_timer_counts ++;
	if(kalman_timer_counts >= 5)                 //500ms进行一次卡尔曼滤波
	{
			// 预测步骤
			kalman_predict(&kf);

//			// 获取带噪声的测量值 (实际中来自定位系统)
//			float true_x = UWB_20_CM->X_cm; // 真实X位置 (沿Y轴运动)
//			float true_y = UWB_20_CM->Y_cm; // 真实Y位置 (0.5m/min = 0.5/60 m/s)

			// 获取带噪声的测量值 (来自定位系统)
			float meas_x = UWB_20_CM->X_cm;
			float meas_y = UWB_20_CM->Y_cm;

			// 更新步骤
			kalman_update(&kf, meas_x, meas_y);

			// 获取滤波后的位置
			kalman_get_position(&kf, &filtered_x, &filtered_y);

			kalman_timer_counts = 0;
	}
}