first commit
This commit is contained in:
lenn
382
src/serial_core/codecs/tactile_a.rs
Normal file
382
src/serial_core/codecs/tactile_a.rs
Normal file
@@ -0,0 +1,382 @@
|
||||
use crate::serial_core::error::CodecError;
|
||||
use crate::serial_core::frame::{
|
||||
FrameHandler, TactileAFrameMetaData, TactileARepFrame, TactileAReqFrame,
|
||||
};
|
||||
use crate::serial_core::record::{write_csv, CsvExporter, CsvImporter, RecordedFrame, Recording};
|
||||
use crate::serial_core::utils::{calc_crc8_itu, elapsed_millis};
|
||||
use crate::serial_core::{
|
||||
codec::Codec,
|
||||
frame::{TactileAFrame, TactileAFrameStatusCode},
|
||||
};
|
||||
use async_trait::async_trait;
|
||||
use csv::StringRecord;
|
||||
use anyhow::anyhow;
|
||||
use std::io::Read;
|
||||
use log::debug;
|
||||
|
||||
const FRAME_BUFFER_MIN_LENGTH: usize = 15;
|
||||
|
||||
pub struct TactileACodec {
|
||||
buffer: Vec<u8>,
|
||||
expected_data_len: usize,
|
||||
}
|
||||
|
||||
pub struct TactileACsvExporter {
|
||||
channels: usize,
|
||||
}
|
||||
|
||||
pub struct TactileACsvImporter {
|
||||
channels: usize,
|
||||
data_row: usize,
|
||||
packets: Vec<TactileADataPacket>,
|
||||
}
|
||||
|
||||
pub struct TactileAHandler;
|
||||
|
||||
#[derive(Clone)]
|
||||
pub struct TactileADataPacket {
|
||||
pub data: Vec<i32>,
|
||||
pub dts_ms: u64,
|
||||
}
|
||||
|
||||
impl From<u8> for TactileAFrameStatusCode {
|
||||
fn from(value: u8) -> Self {
|
||||
match value {
|
||||
0 => TactileAFrameStatusCode::Success,
|
||||
_ => TactileAFrameStatusCode::Failure,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl TryFrom<&TactileARepFrame> for TactileADataPacket {
|
||||
type Error = CodecError;
|
||||
fn try_from(value: &TactileARepFrame) -> Result<TactileADataPacket, Self::Error> {
|
||||
let data = TactileACodec::parse_data_frame(&value.payload)?;
|
||||
let dts_ms = value.dts_ms;
|
||||
Ok(TactileADataPacket {
|
||||
data: data,
|
||||
dts_ms: dts_ms,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl TactileACodec {
|
||||
pub fn new(cols: usize, rows: usize) -> TactileACodec {
|
||||
Self {
|
||||
buffer: Vec::new(),
|
||||
expected_data_len: cols * rows * 2,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn parse_data_frame(data: &[u8]) -> Result<Vec<i32>, CodecError> {
|
||||
if data.len() % 2 != 0 {
|
||||
return Err(CodecError::InvalidLength);
|
||||
}
|
||||
|
||||
let vals: Vec<i32> = data
|
||||
.chunks_exact(2)
|
||||
.map(|chunk| {
|
||||
let raw = u16::from_le_bytes([chunk[0], chunk[1]]) as i32;
|
||||
if raw < 15 {
|
||||
0
|
||||
} else {
|
||||
raw
|
||||
}
|
||||
})
|
||||
.collect::<Vec<i32>>();
|
||||
|
||||
Ok(vals)
|
||||
}
|
||||
|
||||
pub fn build_req_frame(cols: usize, rows: usize) -> anyhow::Result<TactileAFrame> {
|
||||
let header = [0x55, 0xAA];
|
||||
let payload_len: usize = 9;
|
||||
let device_addr: u8 = 0x34;
|
||||
let extend_code: u8 = 0x00;
|
||||
let func_code: u8 = 0xFB;
|
||||
let start_addr: u32 = 7168;
|
||||
let except_data_len: usize = cols * rows * 2;
|
||||
let checksum: u8 = 0;
|
||||
Ok(TactileAFrame::Req(TactileAReqFrame {
|
||||
meta: TactileAFrameMetaData {
|
||||
header,
|
||||
payload_len,
|
||||
device_addr,
|
||||
extend_code,
|
||||
func_code,
|
||||
start_addr,
|
||||
except_data_len,
|
||||
checksum,
|
||||
},
|
||||
}))
|
||||
}
|
||||
}
|
||||
|
||||
impl Codec<TactileAFrame> for TactileACodec {
|
||||
fn decode(
|
||||
&mut self,
|
||||
input: &[u8],
|
||||
session_started_at: std::time::Instant,
|
||||
) -> Result<Vec<TactileAFrame>, CodecError> {
|
||||
self.buffer.extend_from_slice(input);
|
||||
let mut frames: Vec<TactileAFrame> = Vec::new();
|
||||
|
||||
loop {
|
||||
if self.buffer.len() < FRAME_BUFFER_MIN_LENGTH {
|
||||
break;
|
||||
}
|
||||
|
||||
let header_pos = self.buffer.windows(2).position(|w| w == [0xAA, 0x55]);
|
||||
|
||||
let Some(pos) = header_pos else {
|
||||
self.buffer.clear();
|
||||
break;
|
||||
};
|
||||
if pos > 0 {
|
||||
self.buffer.drain(0..pos);
|
||||
}
|
||||
|
||||
if self.buffer.len() < FRAME_BUFFER_MIN_LENGTH {
|
||||
break;
|
||||
}
|
||||
|
||||
let header = [self.buffer[0], self.buffer[1]];
|
||||
let payload_len = u16::from_le_bytes([self.buffer[2], self.buffer[3]]) as usize;
|
||||
let device_addr = self.buffer[4];
|
||||
let extend_code = self.buffer[5];
|
||||
let func_code = self.buffer[6];
|
||||
let start_addr = u32::from_le_bytes([
|
||||
self.buffer[7],
|
||||
self.buffer[8],
|
||||
self.buffer[9],
|
||||
self.buffer[10],
|
||||
]);
|
||||
let except_data_len = u16::from_le_bytes([self.buffer[11], self.buffer[12]]) as usize;
|
||||
let status = TactileAFrameStatusCode::from(self.buffer[13]);
|
||||
if except_data_len != self.expected_data_len {
|
||||
debug!(
|
||||
"unexpected payload length: expected {}, got {}, buffer_len={}",
|
||||
self.expected_data_len,
|
||||
except_data_len,
|
||||
self.buffer.len()
|
||||
);
|
||||
self.buffer.drain(0..1);
|
||||
continue;
|
||||
}
|
||||
|
||||
let frame_length = except_data_len + FRAME_BUFFER_MIN_LENGTH;
|
||||
if self.buffer.len() < frame_length {
|
||||
break;
|
||||
}
|
||||
|
||||
let need_check_data = self.buffer[0..14 + except_data_len].to_vec();
|
||||
let payload = self.buffer[14..14 + except_data_len].to_vec();
|
||||
let crc8_itu_alg = crc::Crc::<u8>::new(&crc::CRC_8_I_432_1);
|
||||
let checksum = crc8_itu_alg.checksum(&need_check_data.as_slice());
|
||||
if self.buffer[frame_length - 1] != checksum {
|
||||
debug!(
|
||||
"checksum mismatch: expected {:02X}, got {:02X}, frame_len={}",
|
||||
checksum,
|
||||
self.buffer[frame_length - 1],
|
||||
frame_length
|
||||
);
|
||||
self.buffer.drain(0..1);
|
||||
continue;
|
||||
}
|
||||
let dts_ms = elapsed_millis(session_started_at);
|
||||
let meta: TactileAFrameMetaData = TactileAFrameMetaData {
|
||||
header,
|
||||
payload_len,
|
||||
device_addr,
|
||||
extend_code,
|
||||
func_code,
|
||||
start_addr,
|
||||
except_data_len,
|
||||
checksum,
|
||||
};
|
||||
frames.push(TactileAFrame::Rep({
|
||||
TactileARepFrame {
|
||||
meta,
|
||||
status,
|
||||
payload,
|
||||
dts_ms,
|
||||
}
|
||||
}));
|
||||
|
||||
self.buffer.drain(0..frame_length);
|
||||
}
|
||||
|
||||
Ok(frames)
|
||||
}
|
||||
|
||||
fn encode(
|
||||
&self,
|
||||
frame: &TactileAFrame,
|
||||
) -> Result<Vec<u8>, crate::serial_core::error::CodecError> {
|
||||
match frame {
|
||||
TactileAFrame::Req(f) => {
|
||||
let mut req_bytes: Vec<u8> = Vec::new();
|
||||
req_bytes.extend_from_slice(f.meta.header.as_slice());
|
||||
req_bytes.extend_from_slice((f.meta.payload_len as u16).to_le_bytes().as_slice());
|
||||
req_bytes.push(f.meta.device_addr);
|
||||
req_bytes.push(f.meta.extend_code);
|
||||
req_bytes.push(f.meta.func_code);
|
||||
|
||||
req_bytes.extend_from_slice(f.meta.start_addr.to_le_bytes().as_slice());
|
||||
req_bytes.extend_from_slice((f.meta.except_data_len as u16).to_le_bytes().as_slice());
|
||||
let checksum = calc_crc8_itu(req_bytes.as_slice());
|
||||
req_bytes.push(checksum);
|
||||
Ok(req_bytes)
|
||||
}
|
||||
_ => {
|
||||
Err(CodecError::InvalidFrameType)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[async_trait]
|
||||
impl FrameHandler<TactileAFrame, i32> for TactileAHandler {
|
||||
async fn on_frame(&mut self, frame: &TactileAFrame) -> anyhow::Result<Option<Vec<i32>>> {
|
||||
match frame {
|
||||
TactileAFrame::Rep(rep) => {
|
||||
let vals = TactileACodec::parse_data_frame(&rep.payload)?;
|
||||
Ok(Some(vals))
|
||||
}
|
||||
_ => Ok(None),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl TactileACsvExporter {
|
||||
fn new(channels: usize) -> Self {
|
||||
TactileACsvExporter { channels }
|
||||
}
|
||||
}
|
||||
|
||||
impl CsvExporter<TactileARepFrame> for TactileACsvExporter {
|
||||
type Error = CodecError;
|
||||
fn csv_header(&self, _recording: &Recording<TactileARepFrame>) -> Vec<String> {
|
||||
let mut header: Vec<String> = Vec::new();
|
||||
for i in 0..self.channels {
|
||||
header.push(format!("channel{}", i + 1));
|
||||
}
|
||||
|
||||
header.push("dts".to_string());
|
||||
header.push("summary".to_string());
|
||||
header
|
||||
}
|
||||
|
||||
fn csv_row(
|
||||
&self,
|
||||
item: &RecordedFrame<TactileARepFrame>,
|
||||
) -> anyhow::Result<Vec<String>> {
|
||||
let packet = TactileADataPacket::try_from(&item.frame)?;
|
||||
let summary: i32 = packet.data.iter().sum();
|
||||
let mut row: Vec<String> = packet.data.iter().map(|x| x.to_string()).collect();
|
||||
row.push(packet.dts_ms.to_string());
|
||||
row.push(summary.to_string());
|
||||
Ok(row)
|
||||
}
|
||||
}
|
||||
|
||||
impl CsvExporter<TactileAFrame> for TactileACsvExporter {
|
||||
type Error = CodecError;
|
||||
|
||||
fn csv_header(&self, _recording: &Recording<TactileAFrame>) -> Vec<String> {
|
||||
let mut header: Vec<String> = Vec::new();
|
||||
for i in 0..self.channels {
|
||||
header.push(format!("channel{}", i + 1));
|
||||
}
|
||||
|
||||
header.push("dts".to_string());
|
||||
header
|
||||
}
|
||||
|
||||
fn csv_row(
|
||||
&self,
|
||||
item: &RecordedFrame<TactileAFrame>,
|
||||
) -> anyhow::Result<Vec<String>> {
|
||||
let rep = match &item.frame {
|
||||
TactileAFrame::Rep(rep) => rep,
|
||||
TactileAFrame::Req(_) => return Err(anyhow!("request frame cannot be exported to csv row")),
|
||||
};
|
||||
|
||||
let packet = TactileADataPacket::try_from(rep)?;
|
||||
let mut row: Vec<String> = packet.data.iter().map(|x| x.to_string()).collect();
|
||||
row.push(packet.dts_ms.to_string());
|
||||
Ok(row)
|
||||
}
|
||||
}
|
||||
|
||||
impl TactileACsvImporter {
|
||||
pub fn new(_path: &str) -> TactileACsvImporter {
|
||||
Self {
|
||||
channels: 0,
|
||||
data_row: 0,
|
||||
packets: Vec::new(),
|
||||
}
|
||||
}
|
||||
|
||||
fn parse_record(&mut self, record: StringRecord) -> anyhow::Result<TactileADataPacket> {
|
||||
if self.channels == 0 {
|
||||
return Err(anyhow!("csv header is missing channel columns"));
|
||||
}
|
||||
|
||||
if record.len() < self.channels + 1 {
|
||||
return Err(anyhow!("csv row has insufficient columns"));
|
||||
}
|
||||
|
||||
let mut data = Vec::with_capacity(self.channels);
|
||||
for index in 0..self.channels {
|
||||
let cell = record.get(index).ok_or_else(|| anyhow!("missing channel cell"))?;
|
||||
data.push(cell.parse::<i32>()?);
|
||||
}
|
||||
|
||||
let dts_cell = record
|
||||
.get(self.channels)
|
||||
.ok_or_else(|| anyhow!("missing dts cell"))?;
|
||||
let dts_ms = dts_cell.parse::<u64>()?;
|
||||
|
||||
Ok(TactileADataPacket {
|
||||
data: data,
|
||||
dts_ms: dts_ms,
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
impl CsvImporter<TactileADataPacket> for TactileACsvImporter {
|
||||
fn load<R: Read>(&mut self, reader: R) -> anyhow::Result<Vec<TactileADataPacket>> {
|
||||
let mut rdr = csv::Reader::from_reader(reader);
|
||||
let headers = rdr.headers()?.clone();
|
||||
self.channels = headers.len().saturating_sub(1);
|
||||
self.data_row = 0;
|
||||
self.packets.clear();
|
||||
|
||||
for record in rdr.records() {
|
||||
let record = record?;
|
||||
let packet = self.parse_record(record)?;
|
||||
self.packets.push(packet);
|
||||
self.data_row += 1;
|
||||
}
|
||||
|
||||
Ok(self.packets.clone())
|
||||
}
|
||||
}
|
||||
|
||||
pub fn export_recording_csv<W>(recording: &Recording<TactileAFrame>, writer: W) -> anyhow::Result<()>
|
||||
where
|
||||
W: std::io::Write,
|
||||
{
|
||||
let channel_nb = recording
|
||||
.frames
|
||||
.iter()
|
||||
.find_map(|frame| match &frame.frame {
|
||||
TactileAFrame::Rep(rep) => Some(rep.payload.len() / 2),
|
||||
TactileAFrame::Req(_) => None,
|
||||
})
|
||||
.unwrap_or(0);
|
||||
|
||||
let exporter = TactileACsvExporter::new(channel_nb);
|
||||
write_csv(recording, &exporter, writer)
|
||||
}
|
||||
Reference in New Issue
Block a user