From 5b795d1dd7c988e94aeb624daf80d834b024bdb8 Mon Sep 17 00:00:00 2001 From: Sameer Puri Date: Tue, 27 Oct 2026 15:33:58 -0700 Subject: [PATCH] Support block stride and ranging intervals in two-way ranging While PR #144 introduced block stride handling and notification intervals for DL-TDoA, Two-Way Ranging ignored BLOCK_STRIDE_LENGTH (0x2D) and still hardcoded current_ranging_interval to 0 in TWR SessionInfoNtf. Unify AppConfig::block_stride across DL-TDoA tags and TWR Controllers, reschedule next_ranging_at on active Session::set_app_config updates, and populate current_ranging_interval in TWR notifications. Closes #149 --- src/app_config.rs | 18 +++- src/device.rs | 2 +- src/lib.rs | 10 +-- src/session.rs | 25 ++++-- tests/ranging.py | 222 ++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 261 insertions(+), 16 deletions(-) diff --git a/src/app_config.rs b/src/app_config.rs index b0d1cbf..63cdf81 100644 --- a/src/app_config.rs +++ b/src/app_config.rs @@ -121,6 +121,7 @@ const DEFAULT_SESSION_DATA_TRANSFER_STATUS_NTF_CONFIG: uci::SessionDataTransferS const DEFAULT_SUSPEND_RANGING_ROUNDS: uci::SuspendRangingRounds = uci::SuspendRangingRounds::Disable; const DEFAULT_DATA_REPETITION_COUNT: u8 = 0; +const DEFAULT_BLOCK_STRIDE_LENGTH: u8 = 0; const DEFAULT_DL_TDOA_BLOCK_SKIPPING: u8 = 0; const DEFAULT_DL_TDOA_ANCHOR_CFO: uci::DlTdoaAnchorCfo = uci::DlTdoaAnchorCfo::AnchorCfoIncluded; const DEFAULT_DL_TDOA_TX_ACTIVE_RANGING_ROUNDS: bool = false; @@ -335,7 +336,7 @@ impl AppConfig { ), ( uci::AppConfigTlvType::BlockStrideLength, - Bytes::from_static(&[0]), + Bytes::from_static(&[DEFAULT_BLOCK_STRIDE_LENGTH]), ), ( uci::AppConfigTlvType::ResultReportConfig, @@ -864,8 +865,19 @@ impl AppConfig { .unwrap_or(DEFAULT_DL_TDOA_BLOCK_SKIPPING) } - pub fn dl_tdoa_block_stride(&self) -> u32 { - u32::from(self.dl_tdoa_block_skipping()) + 1 + fn block_stride_length(&self) -> u8 { + self.get_u8(uci::AppConfigTlvType::BlockStrideLength) + .unwrap_or(DEFAULT_BLOCK_STRIDE_LENGTH) + } + + pub fn block_stride(&self) -> u32 { + match self.dl_tdoa_role() { + Some(uci::DeviceRole::DtTag) => u32::from(self.dl_tdoa_block_skipping()) + 1, + None if self.device_type() == Some(uci::DeviceType::Controller) => { + u32::from(self.block_stride_length()) + 1 + } + _ => 1, + } } fn near_proximity_config(&self) -> u16 { diff --git a/src/device.rs b/src/device.rs index 313ef3c..d767b17 100644 --- a/src/device.rs +++ b/src/device.rs @@ -768,7 +768,7 @@ impl Device { } if invalid_parameters.is_empty() { - session.app_config = app_config; + session.set_app_config(app_config); let effects = if session.state == SessionState::SessionStateInit { session .set_state( diff --git a/src/lib.rs b/src/lib.rs index dc47e4b..0d34eee 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -688,7 +688,7 @@ impl Pica { session_id: u32, session: &Session, ) -> Vec { - let block_stride = session.app_config.dl_tdoa_block_stride(); + let block_stride = session.app_config.block_stride(); let total_blocks = u64::from(session.ranging_count) * u64::from(block_stride); let block_index = total_blocks as u16; let block_ticks = u64::from(session.app_config.ranging_duration()) * TICKS_PER_MS; @@ -1039,8 +1039,8 @@ impl Pica { ShortMacTwoWaySessionInfoNtf { sequence_number: session.sequence_number, session_token: session_id, - rcr_indicator: 0, //TODO - current_ranging_interval: 0, //TODO + rcr_indicator: 0, + current_ranging_interval: session.ranging_interval_ms(), two_way_ranging_measurements: measurements .into_iter() .filter_map(|m| match m { @@ -1057,8 +1057,8 @@ impl Pica { ExtendedMacTwoWaySessionInfoNtf { sequence_number: session.sequence_number, session_token: session_id, - rcr_indicator: 0, //TODO - current_ranging_interval: 0, //TODO + rcr_indicator: 0, + current_ranging_interval: session.ranging_interval_ms(), two_way_ranging_measurements: measurements .into_iter() .filter_map(|m| match m { diff --git a/src/session.rs b/src/session.rs index a5d6f17..967ac56 100644 --- a/src/session.rs +++ b/src/session.rs @@ -212,15 +212,26 @@ impl Session { ) } - fn ranging_interval_ms(&self) -> u32 { - let block_stride = if self.app_config.dl_tdoa_role() == Some(uci::DeviceRole::DtTag) { - self.app_config.dl_tdoa_block_stride() - } else { - 1 - }; + pub fn set_app_config(&mut self, app_config: AppConfig) { + let old_interval = self.ranging_interval(); + self.app_config = app_config; + let new_interval = self.ranging_interval(); + if new_interval != old_interval + && let Some(deadline) = self.next_ranging_at + { + let now = Instant::now(); + self.next_ranging_at = Some( + deadline + .checked_sub(old_interval) + .map_or(now + new_interval, |last| (last + new_interval).max(now)), + ); + } + } + + pub fn ranging_interval_ms(&self) -> u32 { self.app_config .ranging_duration() - .saturating_mul(block_stride) + .saturating_mul(self.app_config.block_stride()) } fn ranging_interval(&self) -> Duration { diff --git a/tests/ranging.py b/tests/ranging.py index 6d09a3d..f57b7c1 100755 --- a/tests/ranging.py +++ b/tests/ranging.py @@ -197,6 +197,7 @@ async def controller( for _ in range(2): event = await host.expect_control(ntf_type, timeout=2.0) event.show() + assert event.current_ranging_interval == 500 assert len(event.two_way_ranging_measurements) == 1 assert event.two_way_ranging_measurements[0].status == uci.Status.OK assert event.two_way_ranging_measurements[0].mac_address == expected_mac @@ -245,6 +246,7 @@ async def controller( event = await host.expect_control(ntf_type, timeout=2.0) event.show() assert event.sequence_number == 2 + assert event.current_ranging_interval == 500 assert len(event.two_way_ranging_measurements) == 1 assert event.two_way_ranging_measurements[0].status == uci.Status.OK assert event.two_way_ranging_measurements[0].mac_address == expected_mac @@ -252,6 +254,7 @@ async def controller( event = await host.expect_control(ntf_type, timeout=2.0) event.show() assert event.sequence_number == 3 + assert event.current_ranging_interval == 500 assert len(event.two_way_ranging_measurements) == 1 assert event.two_way_ranging_measurements[0].status == uci.Status.RANGING_RX_TIMEOUT assert event.two_way_ranging_measurements[0].mac_address == expected_mac @@ -654,6 +657,7 @@ async def controlee( for _ in range(2): event = await host.expect_control(ntf_type, timeout=2.0) event.show() + assert event.current_ranging_interval == 500 assert len(event.two_way_ranging_measurements) == 1 assert event.two_way_ranging_measurements[0].status == uci.Status.OK assert event.two_way_ranging_measurements[0].mac_address == expected_mac @@ -664,6 +668,7 @@ async def controlee( event = await host.expect_control(ntf_type, timeout=2.0) event.show() assert event.sequence_number == 2 + assert event.current_ranging_interval == 500 assert len(event.two_way_ranging_measurements) == 1 assert event.two_way_ranging_measurements[0].status == uci.Status.OK assert event.two_way_ranging_measurements[0].mac_address == expected_mac @@ -994,6 +999,7 @@ async def run(address: str, uci_port: int): await ccc_ranging_session(host0) await aliro_ranging_session(host0) await dl_tdoa_ranging_session(host0, host1, address, uci_port) + await twr_block_stride_ranging(host0, host1) finally: host0.disconnect() host1.disconnect() @@ -1713,6 +1719,222 @@ async def stop_deinit_session(host: Host, sid: int): host_responder.disconnect() +async def twr_block_stride_ranging(host_controller: Host, host_controlee: Host): + sid = 0 + + async def init_twr_session( + host: Host, + role: uci.DeviceRole, + device_type: uci.DeviceType, + peer: Host, + duration_ms: int, + stride: int, + ): + host.send_control( + uci.SessionInitCmd( + session_id=sid, session_type=uci.SessionType.FIRA_RANGING_SESSION + ) + ) + await host.expect_control(uci.SessionInitRsp(status=uci.Status.OK)) + await host.expect_control( + uci.SessionStatusNtf( + session_token=sid, + session_state=uci.SessionState.SESSION_STATE_INIT, + reason_code=0, + ) + ) + host.send_control( + uci.SessionSetAppConfigCmd( + session_token=sid, + tlvs=[ + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.DEVICE_ROLE, + v=bytes([role]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.DEVICE_TYPE, + v=bytes([device_type]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.DEVICE_MAC_ADDRESS, + v=host.mac_address, + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.MULTI_NODE_MODE, + v=bytes([uci.MultiNodeMode.ONE_TO_ONE]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.SCHEDULE_MODE, + v=bytes([uci.ScheduleMode.CONTENTION_BASED]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.RANGING_ROUND_USAGE, + v=bytes([uci.RangingRoundUsage.DS_TWR_DEFERRED_MODE]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.RANGING_DURATION, + v=int.to_bytes(duration_ms, 4, "little"), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.BLOCK_STRIDE_LENGTH, + v=bytes([stride]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.NUMBER_OF_CONTROLEES, + v=bytes([1]), + ), + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.DST_MAC_ADDRESS, + v=peer.mac_address, + ), + ], + ) + ) + await host.expect_control( + uci.SessionSetAppConfigRsp(status=uci.Status.OK, cfg_status=[]) + ) + await host.expect_control( + uci.SessionStatusNtf( + session_token=sid, + session_state=uci.SessionState.SESSION_STATE_IDLE, + reason_code=0, + ) + ) + + async def set_config(host: Host, tlvs: list[uci.AppConfigTlv]): + host.send_control(uci.SessionSetAppConfigCmd(session_token=sid, tlvs=tlvs)) + await host.expect_control( + uci.SessionSetAppConfigRsp(status=uci.Status.OK, cfg_status=[]) + ) + + async def start_session(host: Host): + host.send_control(uci.SessionStartCmd(session_id=sid)) + await host.expect_control(uci.SessionStartRsp(status=uci.Status.OK)) + await host.expect_control( + uci.SessionStatusNtf( + session_token=sid, + session_state=uci.SessionState.SESSION_STATE_ACTIVE, + reason_code=0, + ) + ) + await host.expect_control( + uci.CoreDeviceStatusNtf(device_state=uci.DeviceState.DEVICE_STATE_ACTIVE) + ) + + async def stop_deinit_session(host: Host): + host.send_control(uci.SessionStopCmd(session_id=sid)) + await host.expect_control(uci.SessionStopRsp(status=uci.Status.OK)) + await host.expect_control( + uci.SessionStatusNtf( + session_token=sid, + session_state=uci.SessionState.SESSION_STATE_IDLE, + reason_code=0, + ) + ) + await host.expect_control( + uci.CoreDeviceStatusNtf(device_state=uci.DeviceState.DEVICE_STATE_READY) + ) + host.send_control(uci.SessionDeinitCmd(session_token=sid)) + await host.expect_control(uci.SessionDeinitRsp(status=uci.Status.OK)) + await host.expect_control( + uci.SessionStatusNtf( + session_token=sid, + session_state=uci.SessionState.SESSION_STATE_DEINIT, + reason_code=0, + ) + ) + + await init_twr_session( + host_controlee, + uci.DeviceRole.RESPONDER, + uci.DeviceType.CONTROLEE, + host_controller, + 300, + 2, + ) + await init_twr_session( + host_controller, + uci.DeviceRole.INITIATOR, + uci.DeviceType.CONTROLLER, + host_controlee, + 100, + 1, + ) + + await start_session(host_controlee) + c1 = await host_controlee.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=1.0 + ) + assert c1.current_ranging_interval == 300 + await set_config( + host_controlee, + [ + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.BLOCK_STRIDE_LENGTH, + v=bytes([5]), + ) + ], + ) + c2 = await host_controlee.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=1.0 + ) + assert c2.current_ranging_interval == 300 + await set_config( + host_controlee, + [ + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.SESSION_INFO_NTF_CONFIG, + v=bytes([uci.SessionInfoNtfConfig.DISABLE]), + ) + ], + ) + + await start_session(host_controller) + event1 = await host_controller.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=1.0 + ) + assert event1.current_ranging_interval == 200 + assert event1.two_way_ranging_measurements[0].status == uci.Status.OK + + await set_config( + host_controller, + [ + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.BLOCK_STRIDE_LENGTH, + v=bytes([19]), + ) + ], + ) + with pytest.raises(TimeoutError): + await host_controller.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=0.5 + ) + + await set_config( + host_controller, + [ + uci.AppConfigTlv( + cfg_id=uci.AppConfigTlvType.BLOCK_STRIDE_LENGTH, + v=bytes([0]), + ) + ], + ) + event2 = await host_controller.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=1.0 + ) + assert event2.current_ranging_interval == 100 + assert event2.sequence_number == 1 + + event3 = await host_controller.expect_control( + uci.ShortMacTwoWaySessionInfoNtf, timeout=1.0 + ) + assert event3.current_ranging_interval == 100 + assert event3.sequence_number == 2 + + await stop_deinit_session(host_controller) + await stop_deinit_session(host_controlee) + + def main(): """Start a Pica interactive console.""" parser = argparse.ArgumentParser(description=__doc__)