/*
 * msp_MK32_1.c
 *
 *  Created on: Oct 8, 2024
 *      Author: akeguo
 */
#include "msp_MK32_1.h"
#include "BHBF_ROBOT.h"

char Is_All_Button_Reset = 0;
SP_MSP_MK32_Button *P_MK32;

struct UARTHandler *MK32_Sbus_Controller;

void decode_MK32Data(uint8_t *buffer, uint16_t length)
{
	if (length != 25)
	{
		return;
	}
	if (buffer[0] == 0X0f && buffer[24] == 0X00)
	{
		Sbus_Data_Count(&buffer[1], (int32_t*) (P_MK32));
	}
//	 int32_t RxIndex;
//	    int32_t CH0_RY_H; /* 上1000，下-1000 */
//	    int32_t CH1_RY_V; /* 上1000，下-1000 */
//	    int32_t CH2_LY_V; /* 上1000，下-1000 */
//	    int32_t CH3_LY_H; /* 上1000，下-1000 */
//	    int32_t CH4_SA; /* 上-1000，下1000 */
//	    int32_t CH5_SB; /* 上-1000，下1000 */
//	    int32_t CH6_SC; /* 上-1000，下1000 */
//	    int32_t CH7_SD; /* 上-1000，下1000 */
//	    int32_t CH8_SE; /* 上-1000，下1000 */
//	    int32_t CH9_SF; /* 上-1000，下1000 */
//	    int32_t CH10_LD1;
//	    int32_t CH11_RD1;
//	    int32_t CH12_S1;
//	    int32_t CH13_S2;
//	    int32_t CH14_LT;
//	    int32_t CH15_RT;
//	    int32_t IsOnline;

	if (Is_All_Button_Reset == 0)
	{
		if (P_MK32->CH4_SA == 0 && P_MK32->CH5_SB == 0 && P_MK32->CH6_SC == 0
		        && P_MK32->CH7_SD == 0 && P_MK32->IsOnline == 1)
		{
			HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
			        "Need_To_Reset", 1);
			Is_All_Button_Reset = 1;
		}
	}
	else if (Is_All_Button_Reset == 1)
	{
		HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
		        "Need_To_Reset", 1);
	}

	HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,
	        "mk32_sbus", 1);

}

void Sbus_Data_Count(uint8_t *buf, int32_t *But_Value)
{

	int16_t CH[16];
	int Start_byte = -1;

	CH[0] = ((buf[Start_byte + 1] | buf[Start_byte + 2] << 8) & 0x07FF);
	CH[1] = ((buf[Start_byte + 2] >> 3 | buf[Start_byte + 3] << 5) & 0x07FF);
	CH[2] = ((buf[Start_byte + 3] >> 6 | buf[Start_byte + 4] << 2
	        | buf[Start_byte + 5] << 10) & 0x07FF);
	CH[3] = ((buf[Start_byte + 5] >> 1 | buf[Start_byte + 6] << 7) & 0x07FF);
	CH[4] = ((buf[Start_byte + 6] >> 4 | buf[Start_byte + 7] << 4) & 0x07FF);
	CH[5] = ((buf[Start_byte + 7] >> 7 | buf[Start_byte + 8] << 1
	        | buf[Start_byte + 9] << 9) & 0x07FF);
	CH[6] = ((buf[Start_byte + 9] >> 2 | buf[Start_byte + 10] << 6) & 0x07FF);
	CH[7] = ((buf[Start_byte + 10] >> 5 | buf[Start_byte + 11] << 3) & 0x07FF);
	CH[8] = ((buf[Start_byte + 12] | buf[Start_byte + 13] << 8) & 0x07FF);
	CH[9] = ((buf[Start_byte + 13] >> 3 | buf[Start_byte + 14] << 5) & 0x07FF);
	CH[10] = ((buf[Start_byte + 14] >> 6 | buf[Start_byte + 15] << 2
	        | buf[Start_byte + 16] << 10) & 0x07FF);
	CH[11] = ((buf[Start_byte + 16] >> 1 | buf[Start_byte + 17] << 7) & 0x07FF);
	CH[12] = ((buf[Start_byte + 17] >> 4 | buf[Start_byte + 18] << 4) & 0x07FF);
	CH[13] = ((buf[Start_byte + 18] >> 7 | buf[Start_byte + 19] << 1
	        | buf[Start_byte + 20] << 9) & 0x07FF);
	CH[14] = ((buf[Start_byte + 20] >> 2 | buf[Start_byte + 21] << 6) & 0x07FF);
	CH[15] = ((buf[Start_byte + 21] >> 5 | buf[Start_byte + 22] << 3) & 0x07FF);

	//按键数值：1050为中间值，272-1712
	for (int i = 0; i < 16; i++)
	{
		But_Value[i + 1] = (int32_t) ((CH[i] - 992) * 1.388889);
	}

	//遥控器地面站在线状态
	if (buf[22] == 0) But_Value[17] = 1;
	else
		But_Value[17] = 0;

	But_Value[0]++;
}

void MK32_Sbus_UART_Handler_intialize(struct UARTHandler *Handler)
{
	MK32_Sbus_Controller = Handler;
	MK32_Sbus_Controller->UART_Decode = decode_MK32Data;
	MK32_Sbus_Controller->Wait_time = 4;
	MK32_Sbus_Controller->dispacherController->Dispacher_Enable = 0; //不周期性发送

	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "Need_To_Reset", 0, ComError_MK32_InitialState);
	HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,
	        "mk32_sbus", 0, ComError_Mk32_SBus);
	LOG("MK32_Sbus_intialize");
}