27 lines
25 KiB
XML
27 lines
25 KiB
XML
<?xml version="1.0" encoding="UTF-8"?>
|
||
<project version="4">
|
||
<component name="CopilotDiffPersistence">
|
||
<option name="pendingDiffs">
|
||
<map>
|
||
<entry key="/main (snapshot).rs">
|
||
<value>
|
||
<PendingDiffInfo>
|
||
<option name="filePath" value="/main (snapshot).rs" />
|
||
<option name="originalContent" value="mod policy; mod pingEsp32; mod command; use pingEsp32::Link; use std::time::Duration; fn main() -> std::io::Result<()> { let mut link = Link::open_default()?; link.send_angles(&[1.57;8])?; let mut cmd = command::spawn_listener(); let mut i = 0; while i > -1 { i += 1; if cmd.poll_from_terminal() { let state = cmd.load(); let vec = command::state_to_vector(state); println!("[CommandState] {:?} (state=0b{:03b})", vec, state); } while let Some(msg) = link.recv_timeout(Duration::from_millis(20)) { if msg.is_data() { if let Some(mut obs) = msg.telemetry() { let state = cmd.load(); let cmd_vec = command::state_to_vector(state); obs[6] = cmd_vec[0]; obs[7] = cmd_vec[1]; obs[8] = cmd_vec[2]; println!("收到 36 维观测值(含指令): {:?}", obs); let action = policy::policy(obs).expect("TODO: panic message"); link.send_angles(&action)?; println!("[Action]:{:?}, 【{}】", action, i); } } else if msg.is_reply() { println!("[REPLY] {}", msg.text()); } else { println!("[TEXT] {}", msg.text()); } } std::thread::sleep(Duration::from_millis(5)); } Ok(()) } " />
|
||
<option name="updatedContent" value="mod policy; mod pingEsp32; mod command; use pingEsp32::Link; use std::time::Duration; fn main() -> std::io::Result<()> { let mut link = Link::open_default()?; link.send_angles(&[1.57;8])?; let mut cmd = command::spawn_listener(); let mut i = 0; while i > -1 { i += 1; if let Some(vec) = cmd.poll_from_terminal() { println!("[CommandState] {:?}", vec); if vec == [0.0, 0.0, 0.0] { println!("[Command] released -> zero"); } } while let Some(msg) = link.recv_timeout(Duration::from_millis(20)) { if msg.is_data() { if let Some(mut obs) = msg.telemetry() { let cmd_vec = cmd.current_vector(); obs[6] = cmd_vec[0]; obs[7] = cmd_vec[1]; obs[8] = cmd_vec[2]; println!("收到 36 维观测值(含指令): {:?}", obs); let action = policy::policy(obs).expect("TODO: panic message"); link.send_angles(&action)?; println!("[Action]:{:?}, 【{}】", action, i); } } else if msg.is_reply() { println!("[REPLY] {}", msg.text()); } else { println!("[TEXT] {}", msg.text()); } } std::thread::sleep(Duration::from_millis(5)); } Ok(()) }" />
|
||
</PendingDiffInfo>
|
||
</value>
|
||
</entry>
|
||
<entry key="/pingEsp32 (snapshot).rs">
|
||
<value>
|
||
<PendingDiffInfo>
|
||
<option name="filePath" value="/pingEsp32 (snapshot).rs" />
|
||
<option name="originalContent" value="use std::io::{BufRead, BufReader, Read, Write}; use std::net::TcpStream; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::mpsc::{self, Receiver, Sender, RecvTimeoutError}; use std::sync::Arc; use std::thread::{self, JoinHandle}; use std::time::Duration; use std::f32::consts::PI; pub const ESP32_IP: &str = "192.168.4.1"; pub const ESP32_PORT: u16 = 3333; pub const NUM_SERVOS: usize = 8; const TELEMETRY_MAGIC: [u8; 2] = [0xA5, 0x5A]; const TELEMETRY_FRAME_LEN: usize = 22; /// 从 ESP32 收到的一条消息（可包含 ASCII 文本，也可包含 36 维 IMU 数据） #[derive(Debug, Clone)] pub struct Esp32Message { text: String, telemetry: Option<[f32; 36]>, } impl Esp32Message { pub fn new_text(text: impl Into<String>) -> Self { Self { text: text.into(), telemetry: None, } } pub fn new_telemetry(data: [f32; 36]) -> Self { Self { text: "IMU".to_string(), telemetry: Some(data), } } /// 是否是数据推送（二进制 IMU 帧） pub fn is_data(&self) -> bool { self.telemetry.is_some() } /// 是否是应答，比如 "ESP32: OK ..." pub fn is_reply(&self) -> bool { self.telemetry.is_none() && self.text.starts_with("ESP32:") } pub fn text(&self) -> &str { &self.text } /// 返回 36 维 f32 数组；其余位置按 0.0 填充。 pub fn telemetry(&self) -> Option<[f32; 36]> { self.telemetry } } // ==================== 双向连接 ==================== pub struct Link { writer: TcpStream, rx: Receiver<Esp32Message>, stop: Arc<AtomicBool>, reader_job: Option<JoinHandle<()>>, } impl Link { /// 建立连接并启动后台接收线程 pub fn open(ip: &str, port: u16) -> std::io::Result<Self> { let addr = format!("{}:{}", ip, port); println!("[*] 连接 {} ...", addr); println!("请连接上esp32的wifi热点后再运行本程序【ESP32_AP】"); let stream = TcpStream::connect(&addr)?; stream.set_nodelay(true)?; stream.set_read_timeout(Some(Duration::from_millis(200)))?; println!("[+] 已连接"); let reader_stream = stream.try_clone()?; let (tx, rx): (Sender<Esp32Message>, Receiver<Esp32Message>) = mpsc::channel(); let stop = Arc::new(AtomicBool::new(false)); let reader_job = spawn_reader(reader_stream, tx, Arc::clone(&stop)); Ok(Self { writer: stream, rx, stop, reader_job: Some(reader_job), }) } pub fn open_default() -> std::io::Result<Self> { Self::open(ESP32_IP, ESP32_PORT) } // ---------- 发送 ---------- /// 发送弧度列表 pub fn send_angles(&mut self, rads: &[f32]) -> std::io::Result<()> { let n = rads.len().min(NUM_SERVOS); let payload = rads[..n] .iter() .map(|r| format!("{:.4}", r.clamp(0.0, PI))) .collect::<Vec<_>>() .join(","); self.send_line(&format!("RAD:{}", payload)) } /// 发送角度列表 pub fn send_degrees(&mut self, degs: &[f32]) -> std::io::Result<()> { let n = degs.len().min(NUM_SERVOS); let payload = degs[..n] .iter() .map(|d| format!("{:.2}", d.clamp(0.0, 180.0))) .collect::<Vec<_>>() .join(","); self.send_line(&format!("DEG:{}", payload)) } /// 发送任意一行（自动补 '\n'） pub fn send_line(&mut self, line: &str) -> std::io::Result<()> { self.writer.write_all(line.as_bytes())?; self.writer.write_all(b"\n")?; self.writer.flush() } // ---------- 接收 ---------- /// 阻塞等待一条消息（最多等 timeout） pub fn recv_timeout(&self, timeout: Duration) -> Option<Esp32Message> { match self.rx.recv_timeout(timeout) { Ok(m) => Some(m), Err(RecvTimeoutError::Timeout) => None, Err(RecvTimeoutError::Disconnected) => None, } } /// 非阻塞：立刻返回当前队列里有没有消息 pub fn try_recv(&self) -> Option<Esp32Message> { self.rx.try_recv().ok() } } impl Drop for Link { fn drop(&mut self) { self.stop.store(true, Ordering::SeqCst); let _ = self.writer.shutdown(std::net::Shutdown::Both); if let Some(job) = self.reader_job.take() { let _ = job.join(); } } } // ==================== 后台接收线程 ==================== fn spawn_reader( stream: TcpStream, tx: Sender<Esp32Message>, stop: Arc<AtomicBool>, ) -> JoinHandle<()> { thread::spawn(move || { let mut stream = stream; let mut pending = Vec::new(); let mut buffer = [0u8; 1024]; loop { if stop.load(Ordering::SeqCst) { break; } match stream.read(&mut buffer) { Ok(0) => break, Ok(n) => { pending.extend_from_slice(&buffer[..n]); loop { if pending.len() >= TELEMETRY_FRAME_LEN && pending[..2] == TELEMETRY_MAGIC { let frame = &pending[..TELEMETRY_FRAME_LEN]; if let Some(data) = parse_telemetry_frame(frame) { if tx.send(Esp32Message::new_telemetry(data)).is_err() { return; } pending.drain(..TELEMETRY_FRAME_LEN); continue; } // CRC 错误，丢掉首字节继续找下一帧 pending.drain(..1); continue; } if let Some(newline_pos) = pending.iter().position(|&b| b == b'\n') { let line_bytes = &pending[..newline_pos]; // 只处理纯 ASCII 文本; 非法 UTF-8 也不阻断连接 let line = String::from_utf8_lossy(line_bytes).trim().to_string(); if !line.is_empty() { if tx.send(Esp32Message::new_text(line)).is_err() { return; } } pending.drain(..=newline_pos); continue; } // 还没凑齐完整帧/整行，等下一批数据 break; } if pending.len() > 65536 { pending.clear(); } } Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock || e.kind() == std::io::ErrorKind::TimedOut => { continue; } Err(_) => break, } } println!("[*] 接收线程退出"); }) } fn parse_telemetry_frame(frame: &[u8]) -> Option<[f32; 36]> { if frame.len() != TELEMETRY_FRAME_LEN { return None; } if frame[..2] != TELEMETRY_MAGIC { return None; } let expected_crc = u16::from_be_bytes([frame[20], frame[21]]); let actual_crc = calculate_crc16_ccitt(&frame[..20]); if actual_crc != expected_crc { return None; } let yaw = i16::from_be_bytes([frame[2], frame[3]]) as f32 / 100.0; let pitch = i16::from_be_bytes([frame[4], frame[5]]) as f32 / 100.0; let roll = i16::from_be_bytes([frame[6], frame[7]]) as f32 / 100.0; let ax = i16::from_be_bytes([frame[8], frame[9]]) as f32; let ay = i16::from_be_bytes([frame[10], frame[11]]) as f32; let az = i16::from_be_bytes([frame[12], frame[13]]) as f32; let gx = i16::from_be_bytes([frame[14], frame[15]]) as f32; let gy = i16::from_be_bytes([frame[16], frame[17]]) as f32; let gz = i16::from_be_bytes([frame[18], frame[19]]) as f32; let mut values = [0.0f32; 36]; let payload = [yaw, pitch, roll, ax, ay, az, gx, gy, gz]; for (idx, value) in payload.iter().enumerate() { values[idx] = *value; } Some(values) } fn calculate_crc16_ccitt(data: &[u8]) -> u16 { let mut crc = 0xFFFFu16; for &byte in data { crc ^= (byte as u16) << 8; for _ in 0..8 { if crc & 0x8000 != 0 { crc = ((crc << 1) ^ 0x1021) & 0xFFFF; } else { crc = (crc << 1) & 0xFFFF; } } } crc } " />
|
||
<option name="updatedContent" value="use std::io::{BufRead, BufReader, Read, Write}; use std::net::TcpStream; use std::sync::atomic::{AtomicBool, Ordering}; use std::sync::mpsc::{self, Receiver, Sender, RecvTimeoutError}; use std::sync::Arc; use std::thread::{self, JoinHandle}; use std::time::Duration; use std::f32::consts::PI; pub const ESP32_IP: &str = "192.168.4.1"; pub const ESP32_PORT: u16 = 3333; pub const NUM_SERVOS: usize = 8; const TELEMETRY_MAGIC: [u8; 2] = [0xA5, 0x5A]; const TELEMETRY_FRAME_LEN: usize = 22; /// 从 ESP32 收到的一条消息（可包含 ASCII 文本，也可包含 36 维 IMU 数据） #[derive(Debug, Clone)] pub struct Esp32Message { text: String, telemetry: Option<[f32; 36]>, } impl Esp32Message { pub fn new_text(text: impl Into<String>) -> Self { Self { text: text.into(), telemetry: None, } } pub fn new_telemetry(data: [f32; 36]) -> Self { Self { text: "IMU".to_string(), telemetry: Some(data), } } /// 是否是数据推送（二进制 IMU 帧） pub fn is_data(&self) -> bool { self.telemetry.is_some() } /// 是否是应答，比如 "ESP32: OK ..." pub fn is_reply(&self) -> bool { self.telemetry.is_none() && self.text.starts_with("ESP32:") } pub fn text(&self) -> &str { &self.text } /// 返回 36 维 f32 数组；其余位置按 0.0 填充。 pub fn telemetry(&self) -> Option<[f32; 36]> { self.telemetry } } // ==================== 双向连接 ==================== pub struct Link { writer: TcpStream, rx: Receiver<Esp32Message>, stop: Arc<AtomicBool>, reader_job: Option<JoinHandle<()>>, } impl Link { /// 建立连接并启动后台接收线程 pub fn open(ip: &str, port: u16) -> std::io::Result<Self> { let addr = format!("{}:{}", ip, port); println!("[*] 连接 {} ...", addr); println!("请连接上esp32的wifi热点后再运行本程序【ESP32_AP】"); let stream = TcpStream::connect(&addr)?; stream.set_nodelay(true)?; stream.set_read_timeout(Some(Duration::from_millis(200)))?; println!("[+] 已连接"); let reader_stream = stream.try_clone()?; let (tx, rx): (Sender<Esp32Message>, Receiver<Esp32Message>) = mpsc::channel(); let stop = Arc::new(AtomicBool::new(false)); let reader_job = spawn_reader(reader_stream, tx, Arc::clone(&stop)); Ok(Self { writer: stream, rx, stop, reader_job: Some(reader_job), }) } pub fn open_default() -> std::io::Result<Self> { Self::open(ESP32_IP, ESP32_PORT) } // ---------- 发送 ---------- /// 发送弧度列表 pub fn send_angles(&mut self, rads: &[f32]) -> std::io::Result<()> { let n = rads.len().min(NUM_SERVOS); let payload = rads[..n] .iter() .map(|r| format!("{:.4}", r.clamp(0.0, PI))) .collect::<Vec<_>>() .join(","); self.send_line(&format!("RAD:{}", payload)) } /// 发送角度列表 pub fn send_degrees(&mut self, degs: &[f32]) -> std::io::Result<()> { let n = degs.len().min(NUM_SERVOS); let payload = degs[..n] .iter() .map(|d| format!("{:.2}", d.clamp(0.0, 180.0))) .collect::<Vec<_>>() .join(","); self.send_line(&format!("DEG:{}", payload)) } /// 发送任意一行（自动补 '\n'） pub fn send_line(&mut self, line: &str) -> std::io::Result<()> { self.writer.write_all(line.as_bytes())?; self.writer.write_all(b"\n")?; self.writer.flush() } // ---------- 接收 ---------- /// 阻塞等待一条消息（最多等 timeout） pub fn recv_timeout(&self, timeout: Duration) -> Option<Esp32Message> { match self.rx.recv_timeout(timeout) { Ok(m) => Some(m), Err(RecvTimeoutError::Timeout) => None, Err(RecvTimeoutError::Disconnected) => None, } } /// 非阻塞：立刻返回当前队列里有没有消息 pub fn try_recv(&self) -> Option<Esp32Message> { self.rx.try_recv().ok() } } impl Drop for Link { fn drop(&mut self) { self.stop.store(true, Ordering::SeqCst); let _ = self.writer.shutdown(std::net::Shutdown::Both); if let Some(job) = self.reader_job.take() { let _ = job.join(); } } } // ==================== 后台接收线程 ==================== fn spawn_reader( stream: TcpStream, tx: Sender<Esp32Message>, stop: Arc<AtomicBool>, ) -> JoinHandle<()> { thread::spawn(move || { let mut stream = stream; let mut pending = Vec::new(); let mut buffer = [0u8; 1024]; loop { if stop.load(Ordering::SeqCst) { break; } match stream.read(&mut buffer) { Ok(0) => break, Ok(n) => { pending.extend_from_slice(&buffer[..n]); loop { if pending.len() >= TELEMETRY_FRAME_LEN && pending[..2] == TELEMETRY_MAGIC { let frame = &pending[..TELEMETRY_FRAME_LEN]; if let Some(data) = parse_telemetry_frame(frame) { if tx.send(Esp32Message::new_telemetry(data)).is_err() { return; } pending.drain(..TELEMETRY_FRAME_LEN); continue; } // CRC 错误，丢掉首字节继续找下一帧 pending.drain(..1); continue; } if let Some(newline_pos) = pending.iter().position(|&b| b == b'\n') { let line_bytes = &pending[..newline_pos]; // 只处理纯 ASCII 文本; 非法 UTF-8 也不阻断连接 let line = String::from_utf8_lossy(line_bytes).trim().to_string(); if !line.is_empty() { if tx.send(Esp32Message::new_text(line)).is_err() { return; } } pending.drain(..=newline_pos); continue; } // 还没凑齐完整帧/整行，等下一批数据 break; } if pending.len() > 65536 { pending.clear(); } } Err(ref e) if e.kind() == std::io::ErrorKind::WouldBlock || e.kind() == std::io::ErrorKind::TimedOut => { continue; } Err(_) => break, } } println!("[*] 接收线程退出"); }) } fn parse_telemetry_frame(frame: &[u8]) -> Option<[f32; 36]> { if frame.len() != TELEMETRY_FRAME_LEN { return None; } if frame[..2] != TELEMETRY_MAGIC { return None; } let expected_crc = u16::from_be_bytes([frame[20], frame[21]]); let actual_crc = calculate_crc16_ccitt(&frame[..20]); if actual_crc != expected_crc { return None; } let yaw = i16::from_be_bytes([frame[2], frame[3]]) as f32 / 100.0; let pitch = i16::from_be_bytes([frame[4], frame[5]]) as f32 / 100.0; let roll = i16::from_be_bytes([frame[6], frame[7]]) as f32 / 100.0; let ax = i16::from_be_bytes([frame[8], frame[9]]) as f32; let ay = i16::from_be_bytes([frame[10], frame[11]]) as f32; let az = i16::from_be_bytes([frame[12], frame[13]]) as f32; let gx = i16::from_be_bytes([frame[14], frame[15]]) as f32; let gy = i16::from_be_bytes([frame[16], frame[17]]) as f32; let gz = i16::from_be_bytes([frame[18], frame[19]]) as f32; let mut values = [0.0f32; 36]; let payload = [yaw, pitch, roll, ax, ay, az, gx, gy, gz]; for (idx, value) in payload.iter().enumerate() { values[idx] = *value; } // 对于传回来的向量，按照要求将前三位除以 10000 for i in 0..3 { values[i] /= 10000.0; } Some(values) } fn calculate_crc16_ccitt(data: &[u8]) -> u16 { let mut crc = 0xFFFFu16; for &byte in data { crc ^= (byte as u16) << 8; for _ in 0..8 { if crc & 0x8000 != 0 { crc = ((crc << 1) ^ 0x1021) & 0xFFFF; } else { crc = (crc << 1) & 0xFFFF; } } } crc } " />
|
||
</PendingDiffInfo>
|
||
</value>
|
||
</entry>
|
||
</map>
|
||
</option>
|
||
</component>
|
||
</project> |