VSAG/amadeus_26_fb
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

第一版数据接收

Browse Source
This commit is contained in:
Luo Yufei
2026-03-28 08:48:45 +08:00
parent 141355f1a6
commit 7fe0cb5d10
1 changed files with 128 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

158
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -34,14 +34,111 @@ struct ControlData {
uint8_t right_switch = 3;
};
// 接收的传感器数据
struct SensorData {
float gyro_x = 0.0f; // 陀螺仪x
float accel_x = 0.0f; // 加速度计x
float accel_y = 0.0f; // 加速度计y
float accel_z = 0.0f; // 加速度计z
bool valid = false;
};
static std::mutex control_mutex;
static ControlData shared_control;
static std::mutex sensor_mutex;
static SensorData shared_sensor;
static std::atomic<bool> sender_running{false};
static std::atomic<bool> monitor_running{false};
static std::thread sender_thread;
static std::thread monitor_thread;
// 解析接收到的数据，从杆量位置解算出4个float
// 协议格式：| 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
// 映射关系：x_move -> gyro_x, y_move -> accel_x, yaw -> accel_y, pitch -> accel_z
bool parseSensorData(const uint8_t* data, SensorData& sensor) {
// 检查帧头帧尾
if (data[0] != FRAME_HEADER_1 || data[1] != FRAME_HEADER_2 || data[14] != FRAME_TAIL) {
return false;
}
// 解析4个杆量值（小端序 int16）
int16_t x_move = data[2] | (data[3] << 8);
int16_t y_move = data[4] | (data[5] << 8);
int16_t yaw_val = data[6] | (data[7] << 8);
int16_t pitch_val = data[8] | (data[9] << 8);
// 转换为float（假设原始数据是放大了100倍的）
sensor.gyro_x = x_move / 100.0f;
sensor.accel_x = y_move / 100.0f;
sensor.accel_y = yaw_val / 100.0f;
sensor.accel_z = pitch_val / 100.0f;
sensor.valid = true;
return true;
}
// 获取传感器数据接口
bool getSensorData(SensorData& sensor) {
std::lock_guard<std::mutex> lock(sensor_mutex);
if (shared_sensor.valid) {
sensor = shared_sensor;
return true;
}
return false;
}
// 更新控制数据
void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
uint8_t left_sw = 3, uint8_t right_sw = 3) {
std::lock_guard<std::mutex> lock(control_mutex);
shared_control.yaw = yaw;
shared_control.pitch = pitch;
shared_control.feed = feed;
shared_control.left_switch = left_sw;
shared_control.right_switch = right_sw;
}
// 传感器监控线程 - 独立打印传感器数据
void sensorMonitorLoop() {
LOGM(STR_CTR(WORD_GREEN, "Sensor monitor thread started"));
while (monitor_running) {
SensorData sensor;
if (getSensorData(sensor)) {
// 每秒打印一次传感器数据
LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[Monitor] gyro_x=%.3f, accel_x=%.3f, accel_y=%.3f, accel_z=%.3f"),
sensor.gyro_x, sensor.accel_x, sensor.accel_y, sensor.accel_z);
}
// 每秒打印一次
std::this_thread::sleep_for(std::chrono::seconds(1));
}
LOGM(STR_CTR(WORD_YELLOW, "Sensor monitor thread stopped"));
}
// 启动传感器监控线程
void startSensorMonitor() {
if (!monitor_running) {
monitor_running = true;
monitor_thread = std::thread(sensorMonitorLoop);
}
}
// 停止传感器监控线程
void stopSensorMonitor() {
if (monitor_running) {
monitor_running = false;
if (monitor_thread.joinable()) {
monitor_thread.join();
}
}
}
// 发送线程函数
void senderLoop(Serial &serial) {
uint8_t frame[FRAME_LENGTH];
uint8_t tx_frame[FRAME_LENGTH];
uint8_t rx_frame[FRAME_LENGTH];
while (sender_running) {
auto start = std::chrono::steady_clock::now();
@@ -53,40 +150,49 @@ void senderLoop(Serial &serial) {
local_control = shared_control;
}
// 构建数据帧
// 构建发送数据帧
int idx = 0;
frame[idx++] = FRAME_HEADER_1;
frame[idx++] = FRAME_HEADER_2;
tx_frame[idx++] = FRAME_HEADER_1;
tx_frame[idx++] = FRAME_HEADER_2;
// x_move, y_move (固定为0)
frame[idx++] = 0;
frame[idx++] = 0;
frame[idx++] = 0;
frame[idx++] = 0;
tx_frame[idx++] = 0;
tx_frame[idx++] = 0;
tx_frame[idx++] = 0;
tx_frame[idx++] = 0;
// yaw (小端序)
frame[idx++] = local_control.yaw & 0xFF;
frame[idx++] = (local_control.yaw >> 8) & 0xFF;
tx_frame[idx++] = local_control.yaw & 0xFF;
tx_frame[idx++] = (local_control.yaw >> 8) & 0xFF;
// pitch (小端序)
frame[idx++] = local_control.pitch & 0xFF;
frame[idx++] = (local_control.pitch >> 8) & 0xFF;
tx_frame[idx++] = local_control.pitch & 0xFF;
tx_frame[idx++] = (local_control.pitch >> 8) & 0xFF;
// feed (小端序)
frame[idx++] = local_control.feed & 0xFF;
frame[idx++] = (local_control.feed >> 8) & 0xFF;
tx_frame[idx++] = local_control.feed & 0xFF;
tx_frame[idx++] = (local_control.feed >> 8) & 0xFF;
// switch
frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
tx_frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
// CRC8
frame[idx++] = 0xCC;
tx_frame[idx++] = 0xCC;
// 帧尾
frame[idx++] = FRAME_TAIL;
tx_frame[idx++] = FRAME_TAIL;
// 发送数据
serial.WriteData(frame, FRAME_LENGTH);
serial.WriteData(tx_frame, FRAME_LENGTH);
// 尝试接收数据（非阻塞）
if (serial.ReadData(rx_frame, FRAME_LENGTH)) {
SensorData sensor;
if (parseSensorData(rx_frame, sensor)) {
std::lock_guard<std::mutex> lock(sensor_mutex);
shared_sensor = sensor;
}
}
// 精确控制发送频率
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
@@ -104,18 +210,10 @@ void startSender(Serial &serial) {
sender_running = true;
sender_thread = std::thread(senderLoop, std::ref(serial));
LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
}
}
// 更新控制数据
void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
uint8_t left_sw = 3, uint8_t right_sw = 3) {
std::lock_guard<std::mutex> lock(control_mutex);
shared_control.yaw = yaw;
shared_control.pitch = pitch;
shared_control.feed = feed;
shared_control.left_switch = left_sw;
shared_control.right_switch = right_sw;
// 同时启动传感器监控线程
startSensorMonitor();
}
}
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
@@ -234,7 +332,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
}
// 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
updateControl(150, pitch_scan, 0, 3, 2);
updateControl(300, pitch_scan, 0, 3, 2);
return true;
}
}