432 lines
12 KiB
Rust
432 lines
12 KiB
Rust
use crate::serial_core::codec::Codec;
|
|
use crate::serial_core::codecs::tactile_a::TactileACodec;
|
|
use crate::serial_core::frame::{FrameHandler, TactileAFrame, TestFrame};
|
|
use crate::serial_core::model::{HudChartState, HudPacket};
|
|
#[cfg(feature = "multi-dim")]
|
|
use crate::serial_core::multi_dim_force::PztProcessor;
|
|
use crate::serial_core::record::Recording;
|
|
use crate::serial_core::record::{FrameTiming, RecordedFrame};
|
|
#[cfg(feature = "devkit")]
|
|
use crate::devkit::{proto::SensorFrame, DevKitState};
|
|
use anyhow::Result;
|
|
use log::debug;
|
|
use std::future::pending;
|
|
#[cfg(feature = "devkit")]
|
|
use std::sync::atomic::Ordering;
|
|
use std::sync::{Arc, Mutex};
|
|
use std::time::Instant;
|
|
use tauri::{AppHandle, Emitter};
|
|
#[cfg(feature = "devkit")]
|
|
use tauri::Manager;
|
|
use tokio::io::{AsyncReadExt, AsyncWriteExt};
|
|
use tokio::time::{self, Duration, MissedTickBehavior};
|
|
use tokio_serial::SerialStream;
|
|
use tokio_util::sync::CancellationToken;
|
|
|
|
const AUTO_SUB_INTERVAL: Duration = Duration::from_nanos(16_666_667);
|
|
|
|
pub enum PollMode<F> {
|
|
Disable,
|
|
Enabled(Box<dyn PollRequester<F>>),
|
|
}
|
|
|
|
struct PendingSubFrame<F> {
|
|
frame: F,
|
|
values: Vec<i32>,
|
|
}
|
|
|
|
pub trait SerialFrame: Clone + Send + 'static {
|
|
fn dts_ms(&self) -> u64;
|
|
|
|
fn to_hud_packet(
|
|
&self,
|
|
chart_state: &mut HudChartState,
|
|
display_values: Option<&[i32]>,
|
|
) -> Option<HudPacket>;
|
|
}
|
|
|
|
impl SerialFrame for TestFrame {
|
|
fn dts_ms(&self) -> u64 {
|
|
self.dts_ms
|
|
}
|
|
|
|
fn to_hud_packet(
|
|
&self,
|
|
chart_state: &mut HudChartState,
|
|
display_values: Option<&[i32]>,
|
|
) -> Option<HudPacket> {
|
|
Some(chart_state.apply_frame(self, display_values))
|
|
}
|
|
}
|
|
|
|
impl SerialFrame for TactileAFrame {
|
|
fn dts_ms(&self) -> u64 {
|
|
match self {
|
|
TactileAFrame::Req(_) => 0,
|
|
TactileAFrame::Rep(rep) => rep.dts_ms,
|
|
}
|
|
}
|
|
|
|
fn to_hud_packet(
|
|
&self,
|
|
chart_state: &mut HudChartState,
|
|
display_values: Option<&[i32]>,
|
|
) -> Option<HudPacket> {
|
|
match self {
|
|
TactileAFrame::Req(_) => None,
|
|
TactileAFrame::Rep(rep) => {
|
|
let proxy = TestFrame {
|
|
header: rep.meta.header,
|
|
cmd: rep.meta.func_code,
|
|
length: rep.meta.except_data_len,
|
|
payload: rep.payload.clone(),
|
|
checksum: rep.meta.checksum,
|
|
dts_ms: rep.dts_ms,
|
|
};
|
|
|
|
Some(chart_state.apply_frame(&proxy, display_values))
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pub trait PollRequester<F>: Send {
|
|
fn poll_interval(&self) -> Option<Duration> {
|
|
None
|
|
}
|
|
|
|
fn should_request(&mut self) -> bool {
|
|
true
|
|
}
|
|
|
|
fn next_request(&mut self) -> Result<Option<F>> {
|
|
Ok(None)
|
|
}
|
|
|
|
fn on_rx_frame(&mut self, _frame: &F) {}
|
|
}
|
|
|
|
#[derive(Default)]
|
|
pub struct NoopPollRequester;
|
|
|
|
impl<F> PollRequester<F> for NoopPollRequester {}
|
|
|
|
pub struct TactileAPollRequester {
|
|
period: Duration,
|
|
cols: usize,
|
|
rows: usize,
|
|
awaiting_reply: bool,
|
|
last_request_at: Option<Instant>,
|
|
reply_timeout: Duration,
|
|
}
|
|
|
|
impl TactileAPollRequester {
|
|
pub fn new(period: Duration, cols: usize, rows: usize, reply_timeout: Duration) -> Self {
|
|
Self {
|
|
period,
|
|
cols,
|
|
rows,
|
|
awaiting_reply: false,
|
|
last_request_at: None,
|
|
reply_timeout,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl PollRequester<TactileAFrame> for TactileAPollRequester {
|
|
fn poll_interval(&self) -> Option<Duration> {
|
|
Some(self.period)
|
|
}
|
|
|
|
fn should_request(&mut self) -> bool {
|
|
if !self.awaiting_reply {
|
|
return true;
|
|
}
|
|
let timed_out = self
|
|
.last_request_at
|
|
.map(|t| t.elapsed() >= self.reply_timeout)
|
|
.unwrap_or(false);
|
|
|
|
if timed_out {
|
|
self.awaiting_reply = false;
|
|
self.last_request_at = None;
|
|
return true;
|
|
}
|
|
|
|
false
|
|
}
|
|
|
|
fn next_request(&mut self) -> Result<Option<TactileAFrame>> {
|
|
let req = TactileACodec::build_req_frame(self.cols, self.rows)?;
|
|
self.awaiting_reply = true;
|
|
self.last_request_at = Some(Instant::now());
|
|
Ok(Some(req))
|
|
}
|
|
|
|
fn on_rx_frame(&mut self, frame: &TactileAFrame) {
|
|
if matches!(frame, TactileAFrame::Rep(_)) {
|
|
self.awaiting_reply = false;
|
|
self.last_request_at = None
|
|
}
|
|
}
|
|
}
|
|
|
|
pub async fn run_serial<C, H, T, F>(
|
|
app: AppHandle,
|
|
port: SerialStream,
|
|
codec: C,
|
|
handler: H,
|
|
session_started_at: Instant,
|
|
recording: Arc<Mutex<Recording<F>>>,
|
|
cancel: CancellationToken,
|
|
) -> Result<()>
|
|
where
|
|
F: SerialFrame,
|
|
C: Codec<F> + Send + 'static,
|
|
H: FrameHandler<F, T> + Send + 'static,
|
|
T: Into<i32>,
|
|
{
|
|
run_serial_with_poll(
|
|
app,
|
|
port,
|
|
codec,
|
|
handler,
|
|
session_started_at,
|
|
recording,
|
|
cancel,
|
|
PollMode::Disable,
|
|
)
|
|
.await
|
|
}
|
|
|
|
pub async fn run_serial_with_poll<C, H, T, F>(
|
|
app: AppHandle,
|
|
mut port: SerialStream,
|
|
mut codec: C,
|
|
mut handler: H,
|
|
session_started_at: Instant,
|
|
recording: Arc<Mutex<Recording<F>>>,
|
|
cancel: CancellationToken,
|
|
poll_mode: PollMode<F>,
|
|
) -> Result<()>
|
|
where
|
|
F: SerialFrame,
|
|
C: Codec<F> + Send + 'static,
|
|
H: FrameHandler<F, T> + Send + 'static,
|
|
T: Into<i32>,
|
|
{
|
|
let mut requester = match poll_mode {
|
|
PollMode::Disable => None,
|
|
PollMode::Enabled(r) => Some(r),
|
|
};
|
|
|
|
let mut poll_interval = requester.as_ref().and_then(|r| r.poll_interval()).map(|d| {
|
|
let mut it = time::interval(d);
|
|
it.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
|
it
|
|
});
|
|
let mut poll_sub_interval = time::interval(AUTO_SUB_INTERVAL);
|
|
poll_sub_interval.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
|
|
|
let mut chart_state = HudChartState::new();
|
|
let mut buffer = [0u8; 1024];
|
|
let mut prune_interval = time::interval(Duration::from_millis(450));
|
|
#[cfg(feature = "multi-dim")]
|
|
let mut pzt_processor = PztProcessor::new();
|
|
let mut pending_sub_frame: Option<PendingSubFrame<F>> = None;
|
|
prune_interval.set_missed_tick_behavior(MissedTickBehavior::Delay);
|
|
|
|
loop {
|
|
tokio::select! {
|
|
_ = cancel.cancelled() => break,
|
|
_ = async {
|
|
match poll_interval.as_mut() {
|
|
Some(it) => {
|
|
it.tick().await;
|
|
}
|
|
None => pending::<()>().await,
|
|
}
|
|
} => {
|
|
if let Some(r) = requester.as_mut() {
|
|
if r.should_request() {
|
|
if let Some(req) = r.next_request()? {
|
|
let bytes = codec.encode(&req)?;
|
|
port.write_all(&bytes).await?;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
_ = prune_interval.tick() => {
|
|
if let Some(packet) = chart_state.prune_stale() {
|
|
app.emit("hud_stream", packet)?;
|
|
}
|
|
}
|
|
_ = poll_sub_interval.tick() => {
|
|
if let Some(pending) = pending_sub_frame.take() {
|
|
let display_values = build_display_values(
|
|
&mut chart_state,
|
|
pending.values.as_slice(),
|
|
);
|
|
|
|
if let Some(packet) = pending
|
|
.frame
|
|
.to_hud_packet(&mut chart_state, display_values.as_deref())
|
|
{
|
|
app.emit("hud_stream", packet)?;
|
|
}
|
|
}
|
|
}
|
|
read_result = port.read(&mut buffer) => {
|
|
let n = read_result?;
|
|
if n == 0 {
|
|
// Some serial drivers can resolve reads with 0 bytes repeatedly.
|
|
// Yield here so timer-driven poll requests are not starved by a busy loop.
|
|
tokio::task::yield_now().await;
|
|
continue;
|
|
}
|
|
|
|
let frames = codec.decode(&buffer[..n], session_started_at)?;
|
|
for frame in frames {
|
|
if let Some(r) = requester.as_mut() {
|
|
r.on_rx_frame(&frame);
|
|
}
|
|
|
|
let decode_res = handler
|
|
.on_frame(&frame)
|
|
.await?
|
|
.map(|vals| vals.into_iter().map(Into::into).collect::<Vec<i32>>());
|
|
|
|
let mut record = recording
|
|
.lock()
|
|
.map_err(|_| anyhow::anyhow!("recording state poisoned"))?;
|
|
record.push(RecordedFrame {
|
|
timing: FrameTiming {
|
|
pts_ms: None,
|
|
dts_ms: frame.dts_ms(),
|
|
},
|
|
frame: frame.clone(),
|
|
});
|
|
drop(record);
|
|
|
|
if let Some(vals) = decode_res {
|
|
#[cfg(feature = "multi-dim")]
|
|
{
|
|
let pzt_values = vals.iter().map(|value| *value as f32).collect::<Vec<f32>>();
|
|
if let Ok(angle) = pzt_processor.get_pzt_angle(&pzt_values) {
|
|
// debug!("pzt angle: {:.2}", angle);
|
|
}
|
|
}
|
|
#[cfg(feature = "devkit")]
|
|
{
|
|
let summary = vals.iter().copied().sum::<i32>();
|
|
let force = raw_to_g1(summary as u32);
|
|
push_devkit_frame(&app, vals.as_slice(), frame.dts_ms(), force);
|
|
}
|
|
|
|
pending_sub_frame = Some(PendingSubFrame {
|
|
frame: frame.clone(),
|
|
values: vals,
|
|
});
|
|
} else if let Some(packet) = frame.to_hud_packet(&mut chart_state, None) {
|
|
app.emit("hud_stream", packet)?;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn build_display_values(chart_state: &mut HudChartState, values: &[i32]) -> Option<Vec<i32>> {
|
|
let summary = values.iter().copied().sum::<i32>();
|
|
let force = raw_to_g1(summary as u32);
|
|
chart_state.record_summary(force as f32);
|
|
chart_state.record_pressure_matrix(values);
|
|
Some(vec![summary])
|
|
}
|
|
|
|
#[cfg(feature = "devkit")]
|
|
fn push_devkit_frame(app: &AppHandle, values: &[i32], dts_ms: u64, resultant_force: f64) {
|
|
let devkit_state = app.state::<DevKitState>();
|
|
if !devkit_state.running.load(Ordering::Relaxed) {
|
|
return;
|
|
}
|
|
|
|
let (rows, cols) = infer_matrix_shape(values.len());
|
|
let timestamp_ms = std::time::SystemTime::now()
|
|
.duration_since(std::time::UNIX_EPOCH)
|
|
.unwrap_or_default()
|
|
.as_millis() as u64;
|
|
|
|
let seq = timestamp_ms;
|
|
let matrix = values
|
|
.iter()
|
|
.map(|value| (*value).max(0) as u32)
|
|
.collect::<Vec<_>>();
|
|
|
|
devkit_state.push_frame(SensorFrame {
|
|
seq,
|
|
timestamp_ms,
|
|
rows,
|
|
cols,
|
|
matrix,
|
|
resultant_force,
|
|
dts_ms: dts_ms as u32,
|
|
});
|
|
}
|
|
|
|
#[cfg(feature = "devkit")]
|
|
fn infer_matrix_shape(len: usize) -> (u32, u32) {
|
|
if len == 84 {
|
|
return (12, 7);
|
|
}
|
|
|
|
if len == 0 {
|
|
return (0, 0);
|
|
}
|
|
|
|
let mut best = (len, 1);
|
|
let mut factor = 1usize;
|
|
while factor * factor <= len {
|
|
if len % factor == 0 {
|
|
best = (len / factor, factor);
|
|
}
|
|
factor += 1;
|
|
}
|
|
|
|
(best.0 as u32, best.1 as u32)
|
|
}
|
|
|
|
fn raw_to_g1(raw: u32) -> f64 {
|
|
const X: [u32; 12] = [
|
|
0, 84402, 117218, 140176, 159126, 175812, 191484, 208758, 224703, 252448, 302361, 352703,
|
|
];
|
|
|
|
const Y: [f64; 12] = [
|
|
0.0, 160.0, 260.0, 360.0, 460.0, 560.0, 660.0, 760.0, 860.0, 1060.0, 1560.0, 2060.0,
|
|
];
|
|
|
|
let n = X.len();
|
|
if raw <= X[0] {
|
|
return Y[0] / 100.0;
|
|
}
|
|
if raw >= X[n - 1] {
|
|
return Y[n - 1] / 100.0;
|
|
}
|
|
|
|
let mut left = 0;
|
|
let mut right = n - 1;
|
|
|
|
while left + 1 < right {
|
|
let mid = (left + right) / 2;
|
|
if raw < X[mid] {
|
|
right = mid;
|
|
} else {
|
|
left = mid;
|
|
}
|
|
}
|
|
|
|
let ratio = (raw - X[left]) as f64 / (X[right] - X[left]) as f64;
|
|
Y[left] / 100.0 + ratio * (Y[right] - Y[left]) / 100.0
|
|
}
|