You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
92 lines
2.2 KiB
92 lines
2.2 KiB
1 week ago
|
/*
|
||
|
|
* msp_TL720D.c
|
||
|
|
*
|
||
|
|
* Created on: Jul 19, 2024
|
||
|
|
* Author: bihon
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "msp_TL720D.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
#include "msp_TL720D.pb.h"
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
struct UARTHandler *TL720D_UART_Handler;
|
||
|
|
|
||
|
|
MSP_TL720DParameters* SP_MSP_RF_TL720D_Parameters_In;
|
||
|
|
|
||
|
|
void decode_TL720D(uint8_t *buffer, uint16_t length);
|
||
|
|
int16_t getDeci(uint8_t *data);
|
||
|
|
|
||
|
|
void TL720D_intialize(struct UARTHandler *Handler)
|
||
|
|
{
|
||
|
|
//TL720D_UART_Handler->UART_Decode = NULL;
|
||
|
|
TL720D_UART_Handler = Handler;
|
||
|
|
|
||
|
|
TL720D_UART_Handler->UART_Decode = decode_TL720D;
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"TL720D",0,TL720D);
|
||
|
|
//log_info("TL720D_intialize");
|
||
|
|
|
||
|
|
LOGFF(DL_ERROR,"TL720D_intialize");
|
||
|
|
}
|
||
|
|
void decode_TL720D(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
|
||
|
|
if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00
|
||
|
|
&& buffer[3] == 0x84)
|
||
|
|
{
|
||
|
|
//SP_MSP_RF_TL720D_Parameters_In.
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll = getDeci(&buffer[4]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Pitch = getDeci(&buffer[7]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Yaw = getDeci(&buffer[10]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Acc_X = getDeci(&buffer[13]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Acc_Y = getDeci(&buffer[16]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Acc_Z = getDeci(&buffer[19]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Gro_X = getDeci(&buffer[22]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Gro_Y = getDeci(&buffer[25]);
|
||
|
|
SP_MSP_RF_TL720D_Parameters_In->RF_Gro_Z = getDeci(&buffer[28]);
|
||
|
|
LOG("TL720D decoding succeeded");
|
||
|
|
|
||
|
|
//Is_TL720_Updating_Flag=true;
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"TL720D",1);
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
//log_error("TL720D decoding failed");
|
||
|
|
LOGFF(DL_ERROR,"TL720D decoding failed");
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
int16_t getDeci(uint8_t *data)
|
||
|
|
{
|
||
|
|
char isNegative = 0;
|
||
|
|
|
||
|
|
if (*data >> 4)
|
||
|
|
{
|
||
|
|
isNegative = 1;
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
isNegative = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int16_t data_value = 0;
|
||
|
|
data_value = ((*data) & 0x0f) * 10000;
|
||
|
|
data++;
|
||
|
|
data_value += (*data >> 4) * 1000;
|
||
|
|
data_value += ((*data) & 0x0f) * 100;
|
||
|
|
data++;
|
||
|
|
|
||
|
|
int16_t xiaoshu = 0;
|
||
|
|
xiaoshu = (*data >> 4) * 10;
|
||
|
|
xiaoshu += (*data) & 0x0f;
|
||
|
|
if (isNegative)
|
||
|
|
{
|
||
|
|
return -(data_value + xiaoshu);
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
return (data_value + xiaoshu);
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|