#include "imu948.h"

#define DECLINATION_DEG  -3.0
extern _GPSData gps_data;
extern TX_QUEUE ble_tx_queue;
extern uint8_t rx_byte;
extern TX_EVENT_FLAGS_GROUP sensor_events;

void imu600_init(void)
{
	
	HAL_UART_Receive_IT(&huart3,&rx_byte,1);
	HAL_Delay(6000);
	
	//------- 唤醒传感器，并配置好传感器工作参数，然后开启主动上报-----
	Cmd_03(); //唤醒传感器
	HAL_Delay(500);
	    /**
       * 设置设备参数
     * @param accStill    惯导-静止状态加速度阀值 单位dm/s?
     * @param stillToZero 惯导-静止归零速度(单位cm/s) 0:不归零 255:立即归零
     * @param moveToZero  惯导-动态归零速度(单位cm/s) 0:不归零
     * @param isCompassOn 1=需开启磁场 0=需关闭磁场
     * @param barometerFilter 气压计的滤波等级[取值0-3],数值越大越平稳但实时性越差
     * @param reportHz 数据主动上报的传输帧率[取值0-250HZ], 0表示0.5HZ
     * @param gyroFilter    陀螺仪滤波系数[取值0-2],数值越大越平稳但实时性越差
     * @param accFilter     加速计滤波系数[取值0-4],数值越大越平稳但实时性越差
     * @param compassFilter 磁力计滤波系数[取值0-9],数值越大越平稳但实时性越差
     * @param Cmd_ReportTag 功能订阅标识
     */
    Cmd_12(5, 255, 0,  0, 3, 2, 2, 4, 9, 0xFFF);// 2 设置设备参数(内容1) 只订阅欧拉角
	HAL_Delay(500);
    Cmd_19();// 3 开启数据主动上报   这里我就不进行更改了
}

//void IM948_Init(void)
//{
//    HAL_UART_Receive_IT(&huart3, &rx_byte, 1);

//    // 等模块上电
//    tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND * 5 );


//    Cmd_03();  // 唤醒
//	tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND / 10 );

//    // 只订阅欧拉角
//    Cmd_12(
//        5, 255, 0,
//        0, 3, 5,
//        2, 2, 4,
//        0x0040 // 只要欧拉角
//    );
//	tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND / 10 );

//    Cmd_19(); // 开启主动上报
//}


extern TX_QUEUE im948_uart_rx_queue;
/***
此文件 以IMU.c作为底层代码 Base
现在编写应用层程序
***/

void imu_angle_ble_task_entry(ULONG thread_input)
{
    ULONG rx_data;
	static uint8_t filtInit = 0;
	static float   heading_filt = 0;
	const  float   alpha = 0.20f;               // 20% 一阶低通
    // 初始化模块
//    IM948_Init();
//			HCBle_SendData("halo");
    while(1)
    {
        // 阻塞等队列里有串口接收
        if (tx_queue_receive(&im948_uart_rx_queue, &rx_data, TX_WAIT_FOREVER) == TX_SUCCESS)
        {
            Cmd_GetPkt( (uint8_t)rx_data );
        }

        // 看是否有新包
        if (isNewData)
        {
            isNewData = 0;


            // 这里用angle做盲杖朝向判断
			        /* —— 从 IMU 寄存器/驱动获取融合欧拉角 —— */
			float angleRaw = AngleZ;                   /* 已是度数，磁北基准 */

        /* 转真北    东莞、深圳、广州磁偏角为-3.0°*/
			float heading = angleRaw + DECLINATION_DEG;
			if (heading > 180)  heading -= 360;
			if (heading < -180) heading += 360;

        /* 简单一阶 IIR */
			const float alpha = 0.2f;
			heading_filt += alpha * (heading - heading_filt);

			current_location.angle = heading_filt;
//			current_location.angle = AngleZ * 0.0054931640625f;
			if(current_location.lat != 0 && current_location.lon != 0)
			{
				BleMessage msg = current_location; // 结构体直接拷贝
				tx_queue_send(&ble_tx_queue,&msg,TX_NO_WAIT);
			}
			tx_thread_sleep(10);
        }
    }
}



// 先设置好接收函数
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if(huart->Instance == USART3)
	{
//		HCBle_SendData("RX USART3: 0x%02X (%c)\r\n", rx_byte, rx_byte);

//		Fifo_in(rx_byte);
		ULONG rx_data = rx_byte;
		tx_queue_send(&im948_uart_rx_queue, &rx_data, TX_NO_WAIT);
//		tx_event_flags_set(&sensor_events, EVENT_IMU_DATA_READY, TX_OR);
		//重新启动接收中断，以便继续接收数据
		HAL_UART_Receive_IT(huart,&rx_byte,1);
	}
}