176 lines
5.3 KiB
C
176 lines
5.3 KiB
C
|
||
|
||
/* PS2数据定义
|
||
* BYTE DATA 解释
|
||
* 01 idle
|
||
* 02 0x73 手柄工作模式
|
||
* 03 0x5A Bit0 Bit1 Bit2 Bit3 Bit4 Bit5 Bit6 Bit7
|
||
* 04 data SLCT JOYR JOYL STRT UP RGIHT DOWN L
|
||
* 05 data L2 R2 L1 R1 /\ O X 口
|
||
* 06 data 右边摇杆 0x00 = 左 0xff = 右
|
||
* 07 data 右边摇杆 0x00 = 上 0xff = 下
|
||
* 08 data 左边摇杆 0x00 = 左 0xff = 右
|
||
* 09 data 左边摇杆 0x00 = 上 0xff = 下
|
||
*/
|
||
|
||
#include "PS2.h"
|
||
#include "bsp/delay.h"
|
||
#include "math.h"
|
||
|
||
#define DEADZONE 1.0f // 死区范围（需确保 <127）
|
||
#define EXPO_FACTOR 0.7f // 非线性指数（0=线性，>1=更敏感）
|
||
#define MAX_INPUT 127.0f // 输入最大值（假设为8位有符号，范围[-128,127]）
|
||
#define MIN_INPUT -128.0f
|
||
|
||
uint8_t PS2_RawData[9] = {0}; //存储原始数据的数组
|
||
PS2_TypeDef PS2_Data = {0}; //存储解码后数据的结构体
|
||
|
||
|
||
|
||
void PS2_CS(uint8_t Val)
|
||
{
|
||
if (Val)
|
||
HAL_GPIO_WritePin(PS2_CS_GPIO_Port, PS2_CS_Pin, GPIO_PIN_SET);
|
||
else
|
||
HAL_GPIO_WritePin(PS2_CS_GPIO_Port, PS2_CS_Pin, GPIO_PIN_RESET);
|
||
}
|
||
|
||
|
||
|
||
void PS2_CLK(uint8_t Val)
|
||
{
|
||
if (Val)
|
||
HAL_GPIO_WritePin(PS2_CLK_GPIO_Port, PS2_CLK_Pin, GPIO_PIN_SET);
|
||
else
|
||
HAL_GPIO_WritePin(PS2_CLK_GPIO_Port, PS2_CLK_Pin, GPIO_PIN_RESET);
|
||
}
|
||
|
||
void PS2_DO(uint8_t Val)
|
||
{
|
||
if (Val)
|
||
HAL_GPIO_WritePin(PS2_DO_GPIO_Port, PS2_DO_Pin, GPIO_PIN_SET);
|
||
else
|
||
HAL_GPIO_WritePin(PS2_DO_GPIO_Port, PS2_DO_Pin, GPIO_PIN_RESET);
|
||
}
|
||
|
||
uint8_t PS2_Read_DI()
|
||
{
|
||
return HAL_GPIO_ReadPin(PS2_DI_GPIO_Port, PS2_DI_Pin);
|
||
}
|
||
|
||
uint8_t PS2_ReadWrite_Byte(uint8_t TxData)
|
||
{
|
||
uint8_t TX = TxData;
|
||
uint8_t RX = 0;
|
||
for (int i = 0; i < 8; i++)
|
||
{
|
||
if (TX & 0x01)
|
||
PS2_DO(1);
|
||
else
|
||
PS2_DO(0);
|
||
TX >>= 1;
|
||
PS2_CLK(1);
|
||
BSP_Delay_us(50);
|
||
PS2_CLK(0);
|
||
RX >>= 1;
|
||
RX |= (PS2_Read_DI() << 7);
|
||
BSP_Delay_us(50);
|
||
PS2_CLK(1);
|
||
BSP_Delay_us(50);
|
||
}
|
||
return RX;
|
||
}
|
||
|
||
void PS2_Decode()
|
||
{
|
||
if (PS2_RawData[2] == 0x5A)
|
||
{
|
||
PS2_Data.Key_Select = (~PS2_RawData[3] >> 0) & 0x01; //选择键
|
||
PS2_Data.Key_Start = (~PS2_RawData[3] >> 3) & 0x01; //开始键
|
||
|
||
//左侧按键
|
||
PS2_Data.Key_L_Up = (~PS2_RawData[3] >> 4) & 0x01;
|
||
PS2_Data.Key_L_Right = (~PS2_RawData[3] >> 5) & 0x01;
|
||
PS2_Data.Key_L_Down = (~PS2_RawData[3] >> 6) & 0x01;
|
||
PS2_Data.Key_L_Left = (~PS2_RawData[3] >> 7) & 0x01;
|
||
|
||
//后侧按键
|
||
PS2_Data.Key_L2 = (~PS2_RawData[4] >> 0) & 0x01;
|
||
PS2_Data.Key_R2 = (~PS2_RawData[4] >> 1) & 0x01;
|
||
PS2_Data.Key_L1 = (~PS2_RawData[4] >> 2) & 0x01;
|
||
PS2_Data.Key_R1 = (~PS2_RawData[4] >> 3) & 0x01;
|
||
|
||
//右侧按键
|
||
PS2_Data.Key_R_Up = (~PS2_RawData[4] >> 4) & 0x01;
|
||
PS2_Data.Key_R_Right = (~PS2_RawData[4] >> 5) & 0x01;
|
||
PS2_Data.Key_R_Down = (~PS2_RawData[4] >> 6) & 0x01;
|
||
PS2_Data.Key_R_Left = (~PS2_RawData[4] >> 7) & 0x01;
|
||
|
||
if (PS2_RawData[1] == 0x41)
|
||
{ //无灯模式(摇杆值八向)
|
||
PS2_Data.Rocker_LX = 127 * (PS2_Data.Key_L_Right - PS2_Data.Key_L_Left);
|
||
PS2_Data.Rocker_LY = 127 * (PS2_Data.Key_L_Up - PS2_Data.Key_L_Down);
|
||
|
||
PS2_Data.Rocker_RX = 127 * (PS2_Data.Key_R_Right - PS2_Data.Key_R_Left);
|
||
PS2_Data.Rocker_RY = 127 * (PS2_Data.Key_R_Up - PS2_Data.Key_R_Down);
|
||
}
|
||
else if (PS2_RawData[1] == 0x73)
|
||
{ //红灯模式(摇杆值模拟)
|
||
|
||
//摇杆按键
|
||
PS2_Data.Key_Rocker_Left = (~PS2_RawData[3] >> 1) & 0x01;
|
||
PS2_Data.Key_Rocker_Right = (~PS2_RawData[3] >> 2) & 0x01;
|
||
|
||
//摇杆值
|
||
PS2_Data.Rocker_LX = PS2_RawData[7] - 0x80;
|
||
PS2_Data.Rocker_LY = -1 - (PS2_RawData[8] - 0x80);
|
||
PS2_Data.Rocker_RX = PS2_RawData[5] - 0x80;
|
||
PS2_Data.Rocker_RY = -1 - (PS2_RawData[6] - 0x80);
|
||
}
|
||
}
|
||
}
|
||
void PS2_Read_Data(void)
|
||
{
|
||
PS2_CS(0);
|
||
PS2_RawData[0] = PS2_ReadWrite_Byte(0x01); // 0
|
||
PS2_RawData[1] = PS2_ReadWrite_Byte(0x42); // 1
|
||
for (int i = 2; i < 9; i++)
|
||
PS2_RawData[i] = PS2_ReadWrite_Byte(0xff);
|
||
PS2_CS(1);
|
||
PS2_Decode();
|
||
}
|
||
|
||
float StickToThrottle_Optimized(float stick_value) {
|
||
|
||
if (stick_value >= -DEADZONE && stick_value <= DEADZONE) {
|
||
return 0.5f;
|
||
}
|
||
|
||
float normalized;
|
||
normalized = (stick_value+127.0f)/254.0f;
|
||
if(normalized<0){normalized=0;}
|
||
return normalized;
|
||
}
|
||
|
||
//void UpdateMotorSpeeds(int8_t Rocker_LX, int8_t Rocker_LY, float *motor_speeds) {
|
||
// // 1. 油门映射（带死区和指数曲线）
|
||
// float throttle = StickToThrottle_Optimized(Rocker_LX);
|
||
//
|
||
// // 2. 偏航映射（简单线性，用于差速转向）
|
||
// float yaw = (Rocker_LY + 128.0f) / 256.0f; // [-128,127] -> [0,1]
|
||
//// yaw = yaw * 2.0f - 1.0f; // [0,1] -> [-1,1]
|
||
//
|
||
// // 3. 计算电机转速（示例：四轴无人机）
|
||
// // 假设电机布局为 [前, 右, 后, 左]
|
||
// motor_speeds[0] = throttle; // 前电机（油门直接控制）
|
||
// motor_speeds[1] = throttle * (1.0f - yaw * 0.2f); // 右电机减速转向
|
||
// motor_speeds[2] = throttle; // 后电机
|
||
// motor_speeds[3] = throttle * (1.0f + yaw * 0.2f); // 左电机加速转向
|
||
//
|
||
// // 限制所有电机转速在[0,1]
|
||
// for (int i = 0; i < 4; i++) {
|
||
// motor_speeds[i] = (motor_speeds[i] > 1.0f) ? 1.0f :
|
||
// ((motor_speeds[i] < 0.0f) ? 0.0f : motor_speeds[i]);
|
||
// }
|
||
//}
|