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.
256 lines
7.5 KiB
256 lines
7.5 KiB
2 weeks ago
|
/*
|
||
|
|
* msp_TL720D.c
|
||
|
|
*
|
||
|
|
* Created on: Jul 19, 2024
|
||
|
|
* Author: bihon
|
||
|
|
*
|
||
|
|
* SHI(2026.4)
|
||
|
|
* 倾角仪TL720D与电子罗盘DDM360B，自动输出模式，改为问答模式
|
||
|
|
* 地址为 00 和 02
|
||
|
|
* 波特率 115200
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "MSP/msp_TL720D.h"
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
#include "msp_TL720D.pb.h"
|
||
|
|
|
||
|
|
#define addr_TL720D 0x00//地址
|
||
|
|
#define addr_DDM360B 0x02//地址
|
||
|
|
|
||
|
|
//Roll：0.01°(-180 +180)
|
||
|
|
int32_t *RobotAngle;//机器人角度（陀螺仪TL720D）
|
||
|
|
//Yaw:0.01°(0 360)
|
||
|
|
int32_t *RobotAngle_DDM;//磁北角度（电子罗盘DDM360B）
|
||
|
|
|
||
|
|
|
||
|
|
struct UARTHandler *TL720D_UART_Handler;
|
||
|
|
DispacherController *TL720D_DispacherController;
|
||
|
|
|
||
|
|
MSP_TL720DParameters* SP_MSP_RF_TL720D_Parameters_In;
|
||
|
|
MSP_TL720DParameters* MSP_DDM360B_Parameters_In;
|
||
|
|
|
||
|
|
static void decode_TL720D(uint8_t *buffer, uint16_t length);
|
||
|
|
static void decode_DDM360B(uint8_t *buffer, uint16_t length);
|
||
|
|
static int32_t getDeci(uint8_t *data);
|
||
|
|
static int changeAngle_LeftZerotoUpZero(int angle);
|
||
|
|
|
||
|
|
static void reading_Task_TL720D(void);
|
||
|
|
static void reading_Task_DDM360B(void);
|
||
|
|
|
||
|
|
void TL720D_intialize(struct UARTHandler *Handler)
|
||
|
|
{
|
||
|
|
//TL720D_UART_Handler->UART_Decode = NULL;
|
||
|
|
TL720D_UART_Handler = Handler;
|
||
|
|
TL720D_UART_Handler->Wait_time = 8;
|
||
|
|
// TL720D_UART_Handler->UART_Decode = decode_TL720D;//收到数据时调用decode_TL720D函数进行处理
|
||
|
|
// TL720D_UART_Handler->UART_Decode = decode_DDM360B;
|
||
|
|
|
||
|
|
TL720D_DispacherController = Handler->dispacherController;
|
||
|
|
TL720D_DispacherController->Dispacher_Enable = 1; //不周期性发送
|
||
|
|
// TL720D_DispacherController->DispacherCallTime = 40;//
|
||
|
|
TL720D_DispacherController->DispacherCallTime = 0;//
|
||
|
|
TL720D_DispacherController->Add_Dispatcher_List(TL720D_DispacherController, reading_Task_TL720D);
|
||
|
|
TL720D_DispacherController->Add_Dispatcher_List(TL720D_DispacherController, reading_Task_DDM360B);
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"TL720D",0,ComError_TL720D);//注册硬件错误处理
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"DDM360B",0,ComError_DDM360B);
|
||
|
|
//log_info("TL720D_intialize");
|
||
|
|
LOG("TL720D initialized success");//记录初始化日志
|
||
|
|
LOG("DDM360B initialized success");
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t Buffer_DDM360B[100];//调试用
|
||
|
|
uint8_t Length_DDM360B = 0;//调试用
|
||
|
|
static void decode_DDM360B(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
if(length < 100){
|
||
|
|
Length_DDM360B = length;
|
||
|
|
memcpy(Buffer_DDM360B, buffer, length);//调试用
|
||
|
|
}
|
||
|
|
|
||
|
|
// // DDM360B: 帧长0x0D(13字节), 地址0x02
|
||
|
|
if (buffer[0] == 0x68 && buffer[1] == 0x0D && buffer[2] == 0x02
|
||
|
|
&& buffer[3] == 0x84 && length == 14)
|
||
|
|
{
|
||
|
|
// 验证校验和（不含字头）
|
||
|
|
uint8_t calc_checksum = 0;
|
||
|
|
for (int i = 1; i < (14-1); i++) {
|
||
|
|
calc_checksum += buffer[i];
|
||
|
|
}
|
||
|
|
if (calc_checksum != buffer[14-1]) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
MSP_DDM360B_Parameters_In->RF_Angle_Roll = getDeci(&buffer[4]);
|
||
|
|
MSP_DDM360B_Parameters_In->RF_Angle_Pitch = getDeci(&buffer[7]);
|
||
|
|
MSP_DDM360B_Parameters_In->RF_Angle_Yaw = getDeci(&buffer[10]);
|
||
|
|
*RobotAngle_DDM = MSP_DDM360B_Parameters_In->RF_Angle_Yaw;
|
||
|
|
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"DDM360B",1);
|
||
|
|
LOG("DDM360B decoding succeeded");
|
||
|
|
}else
|
||
|
|
{
|
||
|
|
//log_error("TL720D decoding failed");
|
||
|
|
LOGFF(DL_ERROR,"DDM360B decoding failed");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
static void reading_Task_DDM360B(void)
|
||
|
|
{
|
||
|
|
uint8_t addr = addr_DDM360B;
|
||
|
|
uint8_t cmd[5];
|
||
|
|
uint8_t checksum = 0;
|
||
|
|
|
||
|
|
cmd[0] = 0x68; // 帧头
|
||
|
|
cmd[1] = 0x04; // 数据长度
|
||
|
|
cmd[2] = addr; // 地址码
|
||
|
|
cmd[3] = 0x04; // 命令字（读取角度）
|
||
|
|
|
||
|
|
// 校验和 = 数据长度 + 地址码 + 命令字
|
||
|
|
checksum = cmd[1] + cmd[2] + cmd[3];
|
||
|
|
cmd[4] = checksum;
|
||
|
|
|
||
|
|
memcpy(TL720D_UART_Handler->Tx_Buf, cmd, 5);
|
||
|
|
TL720D_UART_Handler->TxCount = 5;
|
||
|
|
|
||
|
|
TL720D_UART_Handler->AddSendList(
|
||
|
|
TL720D_UART_Handler,
|
||
|
|
TL720D_UART_Handler->Tx_Buf,
|
||
|
|
TL720D_UART_Handler->TxCount,
|
||
|
|
20,
|
||
|
|
decode_DDM360B);
|
||
|
|
// TL720D_UART_Handler->UART_Tx(TL720D_UART_Handler);
|
||
|
|
}
|
||
|
|
|
||
|
|
static void reading_Task_TL720D(void)
|
||
|
|
{
|
||
|
|
// static uint8_t count_read_Task = 0;
|
||
|
|
// count_read_Task++;
|
||
|
|
// if((count_read_Task % 2) == 0)
|
||
|
|
// {
|
||
|
|
// Send_Query_Command(addr_TL720D); // 查询TL720D（地址0x00）
|
||
|
|
// }
|
||
|
|
// else
|
||
|
|
// {
|
||
|
|
// Send_Query_Command(addr_DDM360B); // 查询DDM360B（地址0x02）
|
||
|
|
// }
|
||
|
|
uint8_t addr = addr_TL720D;
|
||
|
|
uint8_t cmd[5];
|
||
|
|
uint8_t checksum = 0;
|
||
|
|
|
||
|
|
cmd[0] = 0x68; // 帧头
|
||
|
|
cmd[1] = 0x04; // 数据长度
|
||
|
|
cmd[2] = addr; // 地址码
|
||
|
|
cmd[3] = 0x04; // 命令字（读取角度）
|
||
|
|
|
||
|
|
// 校验和 = 数据长度 + 地址码 + 命令字
|
||
|
|
checksum = cmd[1] + cmd[2] + cmd[3];
|
||
|
|
cmd[4] = checksum;
|
||
|
|
|
||
|
|
memcpy(TL720D_UART_Handler->Tx_Buf, cmd, 5);
|
||
|
|
TL720D_UART_Handler->TxCount = 5;
|
||
|
|
|
||
|
|
TL720D_UART_Handler->AddSendList(
|
||
|
|
TL720D_UART_Handler,
|
||
|
|
TL720D_UART_Handler->Tx_Buf,
|
||
|
|
TL720D_UART_Handler->TxCount,
|
||
|
|
20,
|
||
|
|
decode_TL720D);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
uint8_t Buffer_TL720D[100];//调试用
|
||
|
|
uint8_t Length_TL720D = 0;//调试用
|
||
|
|
static void decode_TL720D(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
if(length < 100){
|
||
|
|
Length_TL720D = length;
|
||
|
|
memcpy(Buffer_TL720D, buffer, length);//调试用
|
||
|
|
}
|
||
|
|
|
||
|
|
// // TL720D: 帧长0x1F(31字节), 地址0x00
|
||
|
|
if (buffer[0] == 0x68 && buffer[1] == 0x1F && buffer[2] == 0x00
|
||
|
|
&& buffer[3] == 0x84 && length ==32)
|
||
|
|
{
|
||
|
|
// 验证校验和（不含字头）
|
||
|
|
uint8_t calc_checksum = 0;
|
||
|
|
for (int i = 1; i < (32-1); i++) {
|
||
|
|
calc_checksum += buffer[i];
|
||
|
|
}
|
||
|
|
if (calc_checksum != buffer[32-1]) {
|
||
|
|
return; // 校验失败，直接返回
|
||
|
|
}
|
||
|
|
//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]);
|
||
|
|
|
||
|
|
// *RobotAngle = SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll;
|
||
|
|
|
||
|
|
//水平向左为0°，转换为竖直向上为0°;
|
||
|
|
*RobotAngle = changeAngle_LeftZerotoUpZero(SP_MSP_RF_TL720D_Parameters_In->RF_Angle_Roll);
|
||
|
|
|
||
|
|
//Is_TL720_Updating_Flag=true;
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"TL720D",1);
|
||
|
|
LOG("TL720D decoding succeeded");
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
//log_error("TL720D decoding failed");
|
||
|
|
LOGFF(DL_ERROR,"TL720D decoding failed");
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
//水平向左为0°，转换为竖直向上为0°;
|
||
|
|
static int changeAngle_LeftZerotoUpZero(int angle)
|
||
|
|
{
|
||
|
|
int32_t RobotAngle_2 = angle -90*100;
|
||
|
|
if(RobotAngle_2 < -180*100)
|
||
|
|
{
|
||
|
|
RobotAngle_2 = RobotAngle_2 + 360*100;
|
||
|
|
}
|
||
|
|
return RobotAngle_2;
|
||
|
|
}
|
||
|
|
/* 数值计算
|
||
|
|
* data: 数据地址
|
||
|
|
* return: 单位: 0.01
|
||
|
|
* */
|
||
|
|
static int32_t getDeci(uint8_t *data)//int16_t不满足0° ~ 360.00°范围
|
||
|
|
{
|
||
|
|
char isNegative = 0;
|
||
|
|
|
||
|
|
if (*data >> 4)
|
||
|
|
{
|
||
|
|
isNegative = 1;
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
isNegative = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
int32_t data_value = 0;
|
||
|
|
data_value = ((*data) & 0x0f) * 10000;
|
||
|
|
data++;
|
||
|
|
data_value += (*data >> 4) * 1000;
|
||
|
|
data_value += ((*data) & 0x0f) * 100;
|
||
|
|
data++;
|
||
|
|
|
||
|
|
int32_t xiaoshu = 0;
|
||
|
|
xiaoshu = (*data >> 4) * 10;
|
||
|
|
xiaoshu += (*data) & 0x0f;
|
||
|
|
if (isNegative)
|
||
|
|
{
|
||
|
|
return -(data_value + xiaoshu);
|
||
|
|
} else
|
||
|
|
{
|
||
|
|
return (data_value + xiaoshu);
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
//Yaw（航向角）：0° ~ 360.00°，单位 0.01° → 范围 0 ~ 36000 ❌
|
||
|
|
//int16_t 最大只有 32767，但当角度 > 327.67° 时 //就会溢出变成负数
|