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

#include "BHBF_ROBOT.h"


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));

	}
	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]-272)*0.625+1050-1500)*2.22222);

		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;


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