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.
94 lines
2.6 KiB
94 lines
2.6 KiB
1 week ago
|
/*
|
||
|
|
* msp_MK32_1.c
|
||
|
|
*
|
||
|
|
* Created on: Oct 8, 2024
|
||
|
|
* Author: akeguo
|
||
|
|
*/
|
||
|
|
#include "msp_U7.h"
|
||
|
|
#include "DLT/DLTuc.h"
|
||
|
|
|
||
|
|
#include "BHBF_ROBOT.h"
|
||
|
|
|
||
|
|
|
||
|
|
SP_MSP_U7_Button *P_U7;
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
struct UARTHandler *U7_Sbus_Controller;
|
||
|
|
void decode_U7Data(uint8_t *buffer, uint16_t length)
|
||
|
|
{
|
||
|
|
if(length!=25)
|
||
|
|
{
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
if(buffer[0]==0X0f&&buffer[24]==0X00)
|
||
|
|
{
|
||
|
|
Sbus_Data_Count_U7(&buffer[1], (int32_t*) (P_U7));
|
||
|
|
|
||
|
|
}
|
||
|
|
HardWareErrorController->Set_PCOMHardWare(HardWareErrorController,"U7_sbus",1);
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void Sbus_Data_Count_U7(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);
|
||
|
|
|
||
|
|
CH[16] = ((buf[Start_byte + 22] >> 5 | buf[Start_byte + 23] << 3) & 0x07FF);
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
//按键数值：1050为中间值，272-1712
|
||
|
|
for (int i = 0; i < 20; i++) //16
|
||
|
|
{
|
||
|
|
//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]++;
|
||
|
|
if(But_Value[0]>=500){
|
||
|
|
But_Value[0] = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void U7_Sbus_UART_Handler_intialize(struct UARTHandler* Handler)
|
||
|
|
{
|
||
|
|
U7_Sbus_Controller = Handler;
|
||
|
|
U7_Sbus_Controller->UART_Decode=decode_U7Data;
|
||
|
|
|
||
|
|
|
||
|
|
HardWareErrorController->Add_PCOMHardWare(HardWareErrorController,"U7_sbus",0,u7_sbus);
|
||
|
|
LOG("U7_Sbus_intialize");
|
||
|
|
}
|