diff --git a/crates/core/src/bc/codex.rs b/crates/core/src/bc/codex.rs index efba7e6..9a44438 100644 --- a/crates/core/src/bc/codex.rs +++ b/crates/core/src/bc/codex.rs @@ -54,10 +54,17 @@ impl Decoder for BcCodex { if buf.is_empty() { Ok(None) } else { - log::debug!("bytes remaining on BC stream: {:X?}", buf.as_bytes()); + log::debug!( + "bytes remaining on BC stream: {:X?}", + buf.as_bytes().chunks(25).next() + ); // Right after this we seem to get an issue with the camera dropping us // Needs probing - // \xf0\xde\xbc\n\x03\0\0\0@\x9c\0\0\0\x01\x04\0\xc8\0\0\0\0\0\0\0\t\x11\xc6\xa0\x12\xdb\xf7:E\x96* + // F0, DE, BC, A, 3, 0, 0, 0, 88, 6, 0, 0, 0, 1, 4, 0, C8, 0, 0, 0, 0, 0, 0, 0, 30, 31, 64, 63, 48, + // 32, 36, 34, 6A, 6, 0, 0, 0, 0, 0, 0, D8, F5, C7, 86, 56, 0, 0, 0, 0, 0, 0, 1, 21, 9A, FC, 22, 7F, 6, AE, F6, 15, FF, E5, 71, 4, 2F, 24, 61, 15, 96, F0, BF, 83, DE, 10, BE, B4, 2E, 3 + // 9, 76, 56, 92, 7E, 48, 79, 20, 9A, DC, 1B, BB, AC, 22, 60, 5C, 72, B5, 3D, 8, E0, 34, 43, 3F, 2E, A7, 81, A8, 11, 75, 7F, 58, 3E, 8, 54, 91, 43, 21, EC, 6B, D6, 1A, D5, CB, D5, 6C, + // 8C, 2E, 6E, A3, 51, C3, A4, F0, CF, 2B, 61, 81, D0, 1C, A1, 76, EE, BF, 7A, D5, D8, D1, C4, D, B0, 45, EE, 3E, 93, 9A, CE, 5F, AB, 75, 55, AC, 9D, 66, DE, 23, 6D, 5F, 25, 57, DA, F5 + //, E, 7F, 8D, 30, A7, 66, C4, 60, 76, 41, D0, 6A, 23, E, A9, C5, 51, EE, F6, DD, 19, E7, A8, 96, 9F, 2B, AF, 31, 90, 9D, FC, BE Ok(None) } } diff --git a/crates/core/src/bc_protocol/connection/bcconn.rs b/crates/core/src/bc_protocol/connection/bcconn.rs index 770a093..2512770 100644 --- a/crates/core/src/bc_protocol/connection/bcconn.rs +++ b/crates/core/src/bc_protocol/connection/bcconn.rs @@ -5,10 +5,10 @@ use futures::stream::{Stream, StreamExt}; use log::*; use std::collections::btree_map::Entry; use std::collections::BTreeMap; -use std::sync::Arc; use tokio::sync::mpsc::{channel, Sender}; use tokio::task::yield_now; use tokio_stream::wrappers::ReceiverStream; +use tokio_util::sync::PollSender; use tokio::{sync::RwLock, task::JoinSet}; @@ -16,9 +16,9 @@ type MsgHandler = Box Option + Send + Sync>; #[derive(Default)] struct Subscriber { /// Subscribers based on their Num - num: RwLock>>>, + num: BTreeMap>>, /// Subscribers based on their ID - id: RwLock>, + id: BTreeMap, } pub(crate) type BcConnSink = Box + Send + Sync + Unpin>; @@ -31,35 +31,41 @@ pub(crate) type BcConnSource = Box> + Send + Sync + /// There can be only one subscriber per kind of message at a time. pub struct BcConnection { sink: Sender>, - subscribers: Arc, - #[allow(dead_code)] // Not dead we just need to hold a reference to keep it alive + poll_commander: Sender, rx_thread: RwLock>>, } impl BcConnection { - pub async fn new(mut sink: BcConnSink, mut source: BcConnSource) -> Result { - let subscribers: Arc = Default::default(); - let subs = subscribers.clone(); + pub async fn new(mut sink: BcConnSink, source: BcConnSource) -> Result { let (sinker, sinker_rx) = channel::>(100); - let mut rx_thread = JoinSet::new(); - let sink_thread = sinker.clone(); + + let (poll_commander, poll_commanded) = channel(200); + let mut poller = Poller { + subscribers: Default::default(), + sink: sinker.clone(), + reciever: ReceiverStream::new(poll_commanded), + }; + + let mut rx_thread = JoinSet::>::new(); + let thread_poll_commander = poll_commander.clone(); rx_thread.spawn(async move { - loop { - yield_now().await; // Give other tasks a chance to run before pulling next packet - let packet = source.next().await; - let bc = match packet { - Some(res) => res, - None => continue, - }; - Self::poll(bc, &subs, &sink_thread).await? - } + PollSender::new(thread_poll_commander) + .send_all(&mut source.map(|bc| Ok(PollCommand::Bc(Box::new(bc))))) + .await?; + Ok(()) }); rx_thread.spawn(async move { sink.send_all(&mut ReceiverStream::new(sinker_rx)).await }); + rx_thread.spawn(async move { + loop { + poller.run().await?; + } + }); + Ok(BcConnection { sink: sinker, - subscribers, + poll_commander, rx_thread: RwLock::new(rx_thread), }) } @@ -71,18 +77,9 @@ impl BcConnection { pub async fn subscribe(&self, msg_num: u16) -> Result { let (tx, rx) = channel(100); - match self.subscribers.num.write().await.entry(msg_num) { - Entry::Vacant(vac_entry) => { - vac_entry.insert(tx); - } - Entry::Occupied(mut occ_entry) => { - if occ_entry.get().is_closed() { - occ_entry.insert(tx); - } else { - return Err(Error::SimultaneousSubscription { msg_num }); - } - } - }; + self.poll_commander + .send(PollCommand::AddSubscriber(msg_num, tx)) + .await?; Ok(BcSubscription::new(rx, msg_num as u32, self)) } @@ -93,90 +90,18 @@ impl BcConnection { where T: Fn(&Bc) -> Option + Send + Sync + 'static, { - match self.subscribers.id.write().await.entry(msg_id) { - Entry::Vacant(vac_entry) => { - vac_entry.insert(Box::new(handler)); - } - Entry::Occupied(_) => { - return Err(Error::SimultaneousSubscriptionId { msg_id }); - } - }; + self.poll_commander + .send(PollCommand::AddHandler(msg_id, Box::new(handler))) + .await?; Ok(()) } /// Stop a message handler created using [`handle_msg`] #[allow(dead_code)] // Currently unused but added for future use pub async fn unhandle_msg(&self, msg_id: u32) -> Result<()> { - self.subscribers.id.write().await.remove(&msg_id); - Ok(()) - } - - async fn poll( - response: Result, - subscribers: &Subscriber, - sink: &Sender>, - ) -> Result<()> { - match response { - Ok(response) => { - let msg_num = response.meta.msg_num; - let msg_id = response.meta.msg_id; - - let mut remove_it_num = false; - match ( - subscribers.id.read().await.get(&msg_id), - subscribers.num.read().await.get(&msg_num), - ) { - (Some(occ), _) => { - if let Some(reply) = occ(&response) { - assert!(reply.meta.msg_num == response.meta.msg_num); - sink.send(Ok(reply)).await?; - } - } - (None, Some(occ)) => { - if occ.capacity() == 0 { - warn!("Reaching limit of channel"); - warn!( - "Remaining: {} of {} message space for {} (ID: {})", - occ.capacity(), - occ.max_capacity(), - &msg_num, - &msg_id - ); - } else { - trace!( - "Remaining: {} of {} message space for {} (ID: {})", - occ.capacity(), - occ.max_capacity(), - &msg_num, - &msg_id - ); - } - if occ.send(Ok(response)).await.is_err() { - remove_it_num = true; - } - } - (None, None) => { - debug!( - "Ignoring uninteresting message id {} (number: {})", - msg_id, msg_num - ); - trace!("Contents: {:?}", response); - } - } - if remove_it_num { - subscribers.num.write().await.remove(&msg_num); - } - } - Err(e) => { - for sub in subscribers.num.read().await.values() { - let _ = sub.send(Err(e.clone())).await; - } - subscribers.num.write().await.clear(); - subscribers.id.write().await.clear(); - return Err(e); - } - } - + self.poll_commander + .send(PollCommand::RemoveHandler(msg_id)) + .await?; Ok(()) } @@ -198,3 +123,116 @@ impl BcConnection { Ok(()) } } + +enum PollCommand { + Bc(Box>), + AddHandler(u32, MsgHandler), + RemoveHandler(u32), + AddSubscriber(u16, Sender>), +} + +struct Poller { + subscribers: Subscriber, + sink: Sender>, + reciever: ReceiverStream, +} + +impl Poller { + async fn run(&mut self) -> Result<()> { + while let Some(command) = self.reciever.next().await { + yield_now().await; + match command { + PollCommand::Bc(boxed_response) => match *boxed_response { + Ok(response) => { + let msg_num = response.meta.msg_num; + let msg_id = response.meta.msg_id; + + let mut remove_it_num = false; + match ( + self.subscribers.id.get(&msg_id), + self.subscribers.num.get(&msg_num), + ) { + (Some(occ), _) => { + if let Some(reply) = occ(&response) { + assert!(reply.meta.msg_num == response.meta.msg_num); + self.sink.send(Ok(reply)).await?; + } + } + (None, Some(occ)) => { + if occ.capacity() == 0 { + warn!("Reaching limit of channel"); + warn!( + "Remaining: {} of {} message space for {} (ID: {})", + occ.capacity(), + occ.max_capacity(), + &msg_num, + &msg_id + ); + } else { + trace!( + "Remaining: {} of {} message space for {} (ID: {})", + occ.capacity(), + occ.max_capacity(), + &msg_num, + &msg_id + ); + } + if occ.send(Ok(response)).await.is_err() { + remove_it_num = true; + } + } + (None, None) => { + debug!( + "Ignoring uninteresting message id {} (number: {})", + msg_id, msg_num + ); + trace!("Contents: {:?}", response); + } + } + if remove_it_num { + self.subscribers.num.remove(&msg_num); + } + } + Err(e) => { + for sub in self.subscribers.num.values() { + let _ = sub.send(Err(e.clone())).await; + } + self.subscribers.num.clear(); + self.subscribers.id.clear(); + return Err(e); + } + }, + PollCommand::AddHandler(msg_id, handler) => { + match self.subscribers.id.entry(msg_id) { + Entry::Vacant(vac_entry) => { + vac_entry.insert(handler); + } + Entry::Occupied(_) => { + return Err(Error::SimultaneousSubscriptionId { msg_id }); + } + }; + } + PollCommand::RemoveHandler(msg_id) => { + self.subscribers.id.remove(&msg_id); + } + PollCommand::AddSubscriber(msg_num, tx) => { + match self.subscribers.num.entry(msg_num) { + Entry::Vacant(vac_entry) => { + vac_entry.insert(tx); + } + Entry::Occupied(mut occ_entry) => { + if occ_entry.get().is_closed() { + occ_entry.insert(tx); + } else { + let _ = tx + .send(Err(Error::SimultaneousSubscription { msg_num })) + .await; + } + } + }; + } + } + } + Ok(()) + } +} diff --git a/crates/core/src/bc_protocol/errors.rs b/crates/core/src/bc_protocol/errors.rs index d7a5687..5346f7f 100644 --- a/crates/core/src/bc_protocol/errors.rs +++ b/crates/core/src/bc_protocol/errors.rs @@ -208,6 +208,12 @@ impl From> for Error { } } +impl From> for Error { + fn from(_: tokio_util::sync::PollSendError) -> Self { + Error::TokioBcSendError + } +} + impl From for Error { fn from(k: cookie_factory::GenError) -> Self { Error::GenError(std::sync::Arc::new(k)) diff --git a/src/rtsp/gst/sender.rs b/src/rtsp/gst/sender.rs index eae7765..5c1ba75 100644 --- a/src/rtsp/gst/sender.rs +++ b/src/rtsp/gst/sender.rs @@ -20,7 +20,7 @@ use std::{ }; use tokio::{ sync::mpsc::{channel, Receiver, Sender}, - time::Duration, + time::{Duration, Instant}, }; use tokio_stream::wrappers::ReceiverStream; @@ -301,6 +301,11 @@ impl NeoMediaSenders { occ.remove(); continue; } + if let Err(e) = occ.get_mut().stretch_live(&self.buffer).await { + debug!("Could not sretch live: {:?}", e); + occ.remove(); + continue; + } if let Err(e) = occ.get_mut().process_buffer(&self.buffer).await { debug!("Could not send client data: {:?}", e); occ.remove(); @@ -343,6 +348,9 @@ pub(super) enum NeoMediaSenderCommand { #[derive(Debug)] pub(super) struct NeoMediaSender { start_time: FrameTime, + target_start_time: FrameTime, + start_time_v: f32, + start_time_update: Instant, last_sent_time: FrameTime, vid: Option, aud: Option, @@ -364,6 +372,9 @@ impl NeoMediaSender { let (tx, rx) = channel(30); Self { start_time: 0, + target_start_time: 0, + start_time_v: 0.0, + start_time_update: Instant::now(), last_sent_time: 0, vid: None, aud: None, @@ -387,31 +398,35 @@ impl NeoMediaSender { self.command_sender.clone() } - async fn jump_to_live(&mut self, buffer: &NeoBuffer) -> AnyResult<()> { - if self.inited { - let runtime = self.get_runtime().unwrap_or(0); - let (fronts, backs) = buffer.buf.as_slices(); - let frames = fronts.iter().chain(backs.iter()).collect::>(); - let jump_method = JumpMethod::BufferPerunit(0.75); - let target_time = match jump_method { - JumpMethod::MiddleIFrame => { - let idx = frames.len().saturating_div(2); - frames.get(idx).and_then(|f| f.first()).and_then(|f| { - if let BcMedia::Iframe(data) = f { - Some(data.microseconds as FrameTime) - } else { - None - } - }) - } - JumpMethod::BufferPerunit(perunit) => { - if let (Some(st), Some(et)) = (buffer.start_time(), buffer.end_time()) { - Some(((et - st) as f32 * perunit) as FrameTime + st) + async fn target_live_time(&self, buffer: &NeoBuffer) -> AnyResult> { + let (fronts, backs) = buffer.buf.as_slices(); + let frames = fronts.iter().chain(backs.iter()).collect::>(); + let jump_method = JumpMethod::BufferPerunit(0.75); + Ok(match jump_method { + JumpMethod::MiddleIFrame => { + let idx = frames.len().saturating_div(2); + frames.get(idx).and_then(|f| f.first()).and_then(|f| { + if let BcMedia::Iframe(data) = f { + Some(data.microseconds as FrameTime) } else { None } + }) + } + JumpMethod::BufferPerunit(perunit) => { + if let (Some(st), Some(et)) = (buffer.start_time(), buffer.end_time()) { + Some(((et - st) as f32 * perunit) as FrameTime + st) + } else { + None } - }; + } + }) + } + + async fn jump_to_live(&mut self, buffer: &NeoBuffer) -> AnyResult<()> { + if self.inited { + let runtime = self.get_runtime().unwrap_or(0); + let target_time = self.target_live_time(buffer).await?; if let Some(target_time) = target_time { if let Some(et) = buffer.end_time() { if let Ok(delta) = TryInto::::try_into(et - target_time) { @@ -420,6 +435,8 @@ impl NeoMediaSender { } self.start_time = target_time - runtime; + self.target_start_time = self.start_time; + self.start_time_v = 0.0; self.last_sent_time = target_time; debug!( "Target time: {:?}, New start time: {:?}, New Runtime: {:?}, Actual Runtime: {:?}", @@ -431,6 +448,37 @@ impl NeoMediaSender { Ok(()) } + async fn stretch_live(&mut self, buffer: &NeoBuffer) -> AnyResult<()> { + // Desired live time + let target_time = self.target_live_time(buffer).await?; + // Actual live time + let actual_time = self.get_buftime(); + let st = buffer.start_time(); + let et = buffer.end_time(); + if let (Some(target_time), Some(actual_time), Some(st), Some(et)) = + (target_time, actual_time, st, et) + { + if actual_time > st && actual_time < et { + // Only do this while inside the buffer + self.target_start_time += target_time - actual_time; // Adjust + + // Now for the spring + let dt = (Instant::now() - self.start_time_update).as_secs_f32(); + let mut new_start_time = self.start_time as f32; + spring_update( + &mut new_start_time, + self.target_start_time as f32, + &mut self.start_time_v, + 0.5, + dt, + ); + self.start_time = new_start_time as FrameTime; + } + } + self.start_time_update = Instant::now(); + Ok(()) + } + async fn seek(&mut self, target_time: FrameTime) -> AnyResult<()> { self.last_sent_time = self.runtime_to_buftime(target_time); debug!( @@ -665,3 +713,25 @@ impl NeoMediaSender { Ok(true) } } + +const EPS: f32 = 1e-5f32; +fn halflife_to_damping(halflife: f32) -> f32 { + (4.0f32 * std::f32::consts::LN_2) / (halflife + EPS) +} + +// fn damping_to_halflife(damping: f32) -> f32 { +// (4.0f32 * std::f32::consts::LN_2) / (damping + EPS) +// } +fn fast_negexp(x: f32) -> f32 { + 1.0f32 / (1.0f32 + x + 0.48f32 * x * x + 0.235f32 * x * x * x) +} + +fn spring_update(value: &mut f32, target: f32, velocity: &mut f32, halflife: f32, dt: f32) { + let y = halflife_to_damping(halflife) / 2.0; + let j0 = *value - target; + let j1 = *velocity + j0 * y; + let eydt = fast_negexp(y * dt); + + *value = eydt * (j0 + j1 * dt) + target; + *velocity = eydt * (*velocity - j1 * y * dt); +}